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 "thread_adapter.h"
30 # include "navgraph_access_thread.h"
31 # include "navgraph_adapter.h"
35 #include <core/threading/mutex_locker.h>
36 #include <utils/sub_process/proc.h>
37 #include <utils/system/dynamic_module/module.h>
39 #include <AdapterConfiguration.hh>
41 #include <ExecApplication.hh>
42 #include <InterfaceManager.hh>
43 #include <InterfaceSchema.hh>
44 #include <boost/filesystem.hpp>
45 #include <boost/interprocess/sync/file_lock.hpp>
49 #include <pugixml.hpp>
52 namespace fs = boost::filesystem;
64 :
Thread(
"PlexilExecutiveThread",
Thread::OPMODE_CONTINUOUS)
79 std::string cfg_prefix =
"/plexil/" + cfg_spec_ +
"/";
83 std::map<std::string, plexil_interface_config> cfg_adapters =
84 read_plexil_interface_configs(cfg_prefix +
"adapters/");
86 std::map<std::string, plexil_interface_config> cfg_listeners =
87 read_plexil_interface_configs(cfg_prefix +
"listeners/");
89 std::vector<std::string> cfg_lib_path =
92 std::string cfg_basedir =
95 for (
auto &a_item : cfg_adapters) {
96 auto &a = a_item.second;
97 if (a.type ==
"Utility") {
98 logger->
log_warn(
name(),
"Utility adapter configured, consider using FawkesLogging instead");
99 }
else if (a.type ==
"OSNativeTime") {
101 "OSNativeTime adapter configured, consider using FawkesTime instead");
102 }
else if (a.type ==
"FawkesRemoteAdapter") {
104 throw Exception(
"Plexil: cannot load FawkesRemoteAdapter when running internally");
107 std::string filename =
109 if (fs::exists(filename)) {
110 a.attr[
"LibPath"] = filename;
114 plexil_.reset(
new PLEXIL::ExecApplication);
116 PLEXIL::g_manager->setProperty(
"::Fawkes::Config",
config);
117 PLEXIL::g_manager->setProperty(
"::Fawkes::Clock",
clock);
118 PLEXIL::g_manager->setProperty(
"::Fawkes::Logger",
logger);
119 PLEXIL::g_manager->setProperty(
"::Fawkes::BlackBoard",
blackboard);
121 for (
const auto &p : cfg_lib_path) {
122 plexil_->addLibraryPath(p);
125 pugi::xml_document xml_config;
126 pugi::xml_node xml_interfaces =
127 xml_config.append_child(PLEXIL::InterfaceSchema::INTERFACES_TAG());
129 add_plexil_interface_configs(xml_interfaces,
131 PLEXIL::InterfaceSchema::ADAPTER_TAG(),
132 PLEXIL::InterfaceSchema::ADAPTER_TYPE_ATTR());
133 add_plexil_interface_configs(xml_interfaces,
135 PLEXIL::InterfaceSchema::LISTENER_TAG(),
136 PLEXIL::InterfaceSchema::LISTENER_TYPE_ATTR());
138 auto navgraph_adapter_config =
139 std::find_if(cfg_adapters.begin(), cfg_adapters.end(), [](
const auto &entry) {
140 return entry.second.type ==
"NavGraphAdapter";
142 if (navgraph_adapter_config != cfg_adapters.end()) {
145 thread_collector->add(navgraph_access_thread_);
146 navgraph_ = navgraph_access_thread_->get_navgraph();
147 PLEXIL::g_manager->setProperty(
"::Fawkes::NavGraph", &navgraph_);
149 throw Exception(
"NavGraph adapter configured, "
150 "but navgraph library not available at compile time");
155 struct xml_string_writer : pugi::xml_writer
159 write(
const void *data,
size_t size)
161 result.append(
static_cast<const char *
>(data), size);
165 xml_string_writer writer;
166 xml_config.save(writer);
171 std::vector<std::string> debug_markers =
174 std::stringstream dbg_config;
175 for (
const auto &m : debug_markers) {
176 dbg_config << m << std::endl;
178 PLEXIL::readDebugConfigStream(dbg_config);
182 log_stream_.reset(
new std::ostream(&*log_buffer_));
183 PLEXIL::setDebugOutputStream(*log_stream_);
185 if (!plexil_->initialize(xml_interfaces)) {
186 throw Exception(
"Failed to initialize Plexil application");
194 cfg_plan_ple_ = {ple};
197 if (cfg_plan_ple_.empty()) {
201 cfg_plan_auto_compile_ =
203 cfg_plan_force_compile_ =
206 if (!cfg_plan_plx_.empty()) {
207 cfg_plan_plx_ = cfg_basedir +
"/" + cfg_plan_plx_;
208 replace_tokens(cfg_plan_plx_);
211 std::set<std::string> base_paths;
213 for (
auto &p : cfg_plan_ple_) {
214 p = cfg_basedir +
"/" + p;
217 fs::path ple_path{p};
218 fs::path plx_path{fs::path{ple_path}.replace_extension(
".plx")};
221 boost::interprocess::file_lock flock(ple_path.string().c_str());
223 base_paths.insert(plx_path.parent_path().string());
225 if (cfg_plan_auto_compile_) {
226 if (cfg_plan_force_compile_ || !fs::exists(plx_path)
227 || fs::last_write_time(plx_path) < fs::last_write_time(ple_path)) {
229 plexil_compile(ple_path.string());
232 if (!fs::exists(plx_path)) {
233 throw Exception(
"PLX %s does not exist and auto-compile disabled");
234 }
else if (fs::last_write_time(plx_path) < fs::last_write_time(ple_path)) {
236 "PLX %s older than PLE, auto-compile disabled",
237 plx_path.string().c_str());
242 if (!fs::exists(cfg_plan_plx_)) {
243 throw Exception(
"PLX %s does not exist", cfg_plan_plx_.c_str());
246 for (
const auto &p : base_paths) {
247 plexil_->addLibraryPath(p);
250 plan_plx_.reset(
new pugi::xml_document);
251 pugi::xml_parse_result parse_result = plan_plx_->load_file(cfg_plan_plx_.c_str());
252 if (parse_result.status != pugi::status_ok) {
253 throw Exception(
"Failed to parse plan '%s': %s",
254 cfg_plan_plx_.c_str(),
255 parse_result.description());
262 if (!plexil_->startInterfaces()) {
263 throw Exception(
"Failed to start Plexil interfaces");
265 if (!plexil_->run()) {
266 throw Exception(
"Failed to start Plexil");
269 if (!plexil_->addPlan(&*plan_plx_)) {
272 plexil_->notifyExec();
279 if (!plexil_->stop()) {
282 plexil_->notifyExec();
289 if (!plexil_->shutdown()) {
292 PLEXIL::g_configuration->clearAdapterRegistry();
293 plexil_->waitForShutdown();
308 thread_collector->remove(navgraph_access_thread_);
309 delete navgraph_access_thread_;
319 static PLEXIL::ExecApplication::ApplicationState state = PLEXIL::ExecApplication::APP_SHUTDOWN;
320 PLEXIL::ExecApplication::ApplicationState new_state = plexil_->getApplicationState();
321 if (new_state != state) {
322 logger->
log_info(
name(),
"State changed to %s", plexil_->getApplicationStateName(new_state));
326 using namespace std::chrono_literals;
327 std::this_thread::sleep_for(500ms);
331 std::map<std::string, PlexilExecutiveThread::plexil_interface_config>
332 PlexilExecutiveThread::read_plexil_interface_configs(
const std::string &config_prefix)
334 std::map<std::string, plexil_interface_config> cfg_adapters;
336 std::unique_ptr<Configuration::ValueIterator> cfg_item{
config->
search(config_prefix)};
337 while (cfg_item->next()) {
338 std::string path = cfg_item->
path();
340 std::string::size_type start_pos = config_prefix.size();
341 std::string::size_type slash_pos = path.find(
"/", start_pos + 1);
342 if (slash_pos != std::string::npos) {
343 std::string
id = path.substr(start_pos, slash_pos - start_pos);
345 start_pos = slash_pos + 1;
346 slash_pos = path.find(
"/", start_pos);
347 std::string what = path.substr(start_pos, slash_pos - start_pos);
349 if (what ==
"type") {
350 cfg_adapters[id].type = cfg_item->get_string();
351 }
else if (what ==
"attr") {
352 start_pos = slash_pos + 1;
353 slash_pos = path.find(
"/", start_pos);
354 std::string key = path.substr(start_pos, slash_pos - start_pos);
355 cfg_adapters[id].attr[key] = cfg_item->get_as_string();
356 }
else if (what ==
"args") {
357 start_pos = slash_pos + 1;
358 slash_pos = path.find(
"/", start_pos);
359 std::string key = path.substr(start_pos, slash_pos - start_pos);
360 cfg_adapters[id].args[key] = cfg_item->get_as_string();
361 }
else if (what ==
"verbatim-args") {
362 start_pos = slash_pos + 1;
363 slash_pos = path.find(
"/", start_pos);
364 std::string verb_id = path.substr(start_pos, slash_pos - start_pos);
366 start_pos = slash_pos + 1;
367 slash_pos = path.find(
"/", start_pos);
368 std::string verb_what = path.substr(start_pos, slash_pos - start_pos);
370 if (verb_what ==
"tag") {
371 cfg_adapters[id].verbatim_args[verb_id].tag = cfg_item->get_as_string();
372 }
else if (verb_what ==
"text") {
373 cfg_adapters[id].verbatim_args[verb_id].has_text =
true;
374 cfg_adapters[id].verbatim_args[verb_id].text = cfg_item->get_as_string();
375 }
else if (verb_what ==
"attr") {
376 start_pos = slash_pos + 1;
377 slash_pos = path.find(
"/", start_pos);
378 std::string verb_key = path.substr(start_pos, slash_pos - start_pos);
379 cfg_adapters[id].verbatim_args[verb_id].attr[verb_key] = cfg_item->get_as_string();
381 }
else if (what ==
"verbatim-xml") {
383 pugi::xml_parse_result parse_result =
384 cfg_adapters[id].verbatim.load_string(cfg_item->get_string().c_str());
385 if (parse_result.status != pugi::status_ok) {
386 throw Exception(
"Failed to parse verbatim-xml for '%s': %s",
387 cfg_adapters[
id].type.c_str(),
388 parse_result.description());
398 PlexilExecutiveThread::add_plexil_interface_configs(
399 pugi::xml_node & parent,
400 const std::map<std::string, PlexilExecutiveThread::plexil_interface_config> &configs,
401 const char * tag_name,
402 const char * type_attr_name)
404 for (
const auto &a_item : configs) {
405 const auto & a = a_item.second;
406 pugi::xml_node xml_adapter = parent.append_child(tag_name);
407 xml_adapter.append_attribute(type_attr_name).set_value(a.type.c_str());
408 for (
const auto &attr : a.attr) {
409 xml_adapter.append_attribute(attr.first.c_str()).set_value(attr.second.c_str());
411 for (
const auto &arg : a.args) {
412 pugi::xml_node xml_adapter_arg = xml_adapter.append_child(
"Parameter");
413 xml_adapter_arg.append_attribute(
"key").set_value(arg.first.c_str());
414 xml_adapter_arg.text().set(arg.second.c_str());
416 for (
const auto &arg : a.verbatim_args) {
417 const auto & varg = arg.second;
418 pugi::xml_node xml_adapter_arg = xml_adapter.append_child(varg.tag.c_str());
419 for (
const auto &attr : varg.attr) {
420 xml_adapter_arg.append_attribute(attr.first.c_str()).set_value(attr.second.c_str());
423 xml_adapter_arg.text().set(varg.text.c_str());
426 if (a.verbatim && a.verbatim.children().begin() != a.verbatim.children().end()) {
427 for (
const auto &child : a.verbatim.children()) {
428 xml_adapter.append_copy(child);
435 PlexilExecutiveThread::plexil_compile(
const std::string &ple_file)
437 std::vector<std::string> argv{
"plexilc", ple_file};
438 std::string command_line =
439 std::accumulate(std::next(argv.begin()),
442 [](std::string &s,
const std::string &a) { return s +
" " + a; });
446 using namespace std::chrono_literals;
447 auto compile_start = std::chrono::system_clock::now();
448 auto now = std::chrono::system_clock::now();
453 throw Exception(
"Plexil compilation failed, check log for messages.");
458 now = std::chrono::system_clock::now();
459 std::this_thread::sleep_for(500ms);
460 }
while (now < compile_start + 30s);
463 throw Exception(
"Plexil compilation timeout after 30s");
virtual void init()
Initialize the thread.
virtual ~PlexilExecutiveThread()
Destructor.
PlexilExecutiveThread()
Constructor.
virtual void loop()
Code to execute in the thread.
virtual void finalize()
Finalize the thread.
virtual bool prepare_finalize_user()
Prepare finalization user implementation.
virtual void once()
Execute an action exactly once.
Log Plexil log output to Fawkes logger.
Access to internal navgraph for Plexil.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Clock * clock
By means of this member access to the clock is given.
Configuration * config
This is the Configuration member used to access the configuration.
virtual const char * path() const =0
Path of value.
virtual std::string get_string_or_default(const char *path, const std::string &default_val)
Get value from configuration which is of type string, or the given default if the path does not exist...
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
virtual bool is_list(const char *path)=0
Check if a value is a list.
virtual bool get_bool_or_default(const char *path, const bool &default_val)
Get value from configuration which is of type bool, or the given default if the path does not exist.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
virtual std::vector< std::string > get_strings_or_defaults(const char *path, const std::vector< std::string > &default_val)
Get list of values from configuration which is of type string, or the given default if the path does ...
Base class for exceptions in Fawkes.
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.
Logger * logger
This is the Logger member used to access the logger.
static const char * get_file_extension()
Get file extension for dl modules.
Sub-process execution with stdin/stdout/stderr redirection.
bool alive()
Check if process is alive.
int exit_status()
Get exit status of process once it ended.
void check_proc()
Check if the process is still alive.
void kill(int signum)
Send a signal to the process.
Thread class encapsulation of pthreads.
void set_prepfin_conc_loop(bool concurrent=true)
Set concurrent execution of prepare_finalize() and loop().
const char * name() const
Get name of thread.
Fawkes library namespace.