Fawkes API  Fawkes Development Version
skiller_action_executor.cpp
1 /***************************************************************************
2  * skiller_action_executor.cpp - Execute skills for Golog++ activities
3  *
4  * Created: Thu 03 Oct 2019 08:58:22 CEST 08:58
5  * Copyright 2019 Till Hofmann <hofmann@kbsg.rwth-aachen.de>
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include "skiller_action_executor.h"
22 
23 #include <blackboard/blackboard.h>
24 #include <config/config.h>
25 #include <golog++/model/activity.h>
26 #include <interfaces/SkillerInterface.h>
27 #include <logging/logger.h>
28 
29 namespace fawkes {
30 namespace gpp {
31 
32 using gologpp::Transition;
33 
34 /** @class InvalidArgumentException
35  * An exception that is thrown if the given arguments do not match the skill's arguments.
36  */
37 
38 /** Constructor.
39  * @param format A format string for the format message
40  */
42 {
43  va_list args;
44  va_start(args, format);
45  append_nolock_va(format, args);
46  va_end(args);
47 }
48 
49 /** @class SkillerActionExecutor
50  * An ActionExecutor that executes an activity using the Skiller.
51  * An action is translated to a skill using the skill mapping from the configuration.
52  * If the Skiller's status changes, the activity's status is updated accordingly.
53  * @author Till Hofmann
54  * @see ActionSkillMapping
55  *
56  * @var SkillerActionExecutor::blackboard_
57  * The blackboard to use to access the skiller.
58  *
59  * @var SkillerActionExecutor::blackboard_owner_
60  * True if this executor is owning its blackboard.
61  */
62 
63 /** Constructor.
64  * Create and initialize the executor, so a subsequent call to start() directly
65  * starts executing a skill.
66  * @param logger A logger instance to use
67  * @param blackboard The blackboard to use to connect to the SkillerInterface
68  * @param config The config to read the skill mapping from
69  * @param cfg_prefix The spec-specific config prefix to use
70  */
72  BlackBoard * blackboard,
73  Configuration * config,
74  const std::string &cfg_prefix)
75 : ActionExecutor(logger),
76  BlackBoardInterfaceListener("Golog++SkillerActionExecutor"),
77  blackboard_(blackboard),
78  blackboard_owner_(false),
79  config_(config),
80  cfg_prefix_(cfg_prefix)
81 {
82  try {
83  skiller_if_ = blackboard_->open_for_reading<SkillerInterface>("Skiller");
84  } catch (Exception &e) {
85  logger_->log_error(name(), "Failed to open skiller interface: %s", e.what_no_backtrace());
86  }
87  bbil_add_data_interface(skiller_if_);
89  skiller_if_->read();
90  if (!skiller_if_->has_writer()) {
91  blackboard->unregister_listener(this);
92  blackboard->close(skiller_if_);
93  throw Exception("No writer for Skiller interface");
94  }
96  initialize_action_skill_mapping();
97 }
98 
99 void
100 SkillerActionExecutor::initialize_action_skill_mapping()
101 {
102  std::string action_mapping_cfg_path = cfg_prefix_ + "/action-mapping/";
103  auto cfg_iterator{config_->search(action_mapping_cfg_path)};
104  std::map<std::string, std::string> mapping;
105  while (cfg_iterator->next()) {
106  std::string action_name{
107  std::string(cfg_iterator->path()).substr(action_mapping_cfg_path.length())};
108  mapping[action_name] = cfg_iterator->get_as_string();
109  }
110  action_skill_mapping_ = ActionSkillMapping(mapping);
111 }
112 
113 /** Destructor.
114  * Close all interfaces and unregister from the blackboard.
115  */
117 {
120  blackboard_->close(skiller_if_);
121  if (blackboard_owner_) {
122  delete blackboard_;
123  }
124 }
125 
126 /** Check if we can execute the given activity.
127  * Check the action skill mapping whether the given action occurs in the mapping.
128  * If not, we cannot execute the activity.
129  * @param activity An activity to execute
130  * @return true iff the given activity can be executed by this executor
131  */
132 bool
133 SkillerActionExecutor::can_execute_activity(std::shared_ptr<gologpp::Activity> activity) const
134 {
135  return action_skill_mapping_.has_mapping(activity->mapped_name());
136 }
137 
138 /** Start the given activity.
139  * Instruct the skiller to execute the activity.
140  * @param activity the activity to execute
141  */
142 void
143 SkillerActionExecutor::start(std::shared_ptr<gologpp::Activity> activity)
144 {
145  if (!can_execute_activity(activity)) {
146  throw Exception("Cannot execute activity '%s' with SkillerActionExecutor",
147  activity->mapped_name().c_str());
148  }
149  try {
150  skiller_if_->msgq_enqueue(
151  new SkillerInterface::ExecSkillMessage(map_activity_to_skill(activity).c_str()));
152  running_activity_ = activity;
153  } catch (InvalidArgumentException &e) {
154  logger_->log_error(name(), "Failed to start %s: %s", activity->name().c_str(), e.what());
155  activity->update(Transition::Hook::FAIL);
156  }
157 }
158 
159 /** Stop the activity if it is currently running.
160  * If the given activity does not match the currently running activity, do nothing.
161  * @param activity The activity to stop
162  */
163 void
164 SkillerActionExecutor::stop(std::shared_ptr<gologpp::Grounding<gologpp::Action>> activity)
165 {
166  if (*running_activity_ == *activity) {
168  running_activity_.reset();
169  }
170 }
171 
172 /** Get the name of the executor; mainly used for logging.
173  * @return The human-readable name of the executor
174  */
175 const char *
177 {
178  return "SkillerActionExecutor";
179 }
180 
181 std::string
182 SkillerActionExecutor::map_activity_to_skill(std::shared_ptr<gologpp::Activity> activity)
183 {
184  std::map<std::string, std::string> params;
185  for (auto &arg : activity->target()->mapping().arg_mapping()) {
186  try {
187  params[arg.first] = static_cast<std::string>(activity->mapped_arg_value(arg.first));
188  } catch (boost::bad_get &e) {
189  throw InvalidArgumentException("Failed to cast parameter %s: %s",
190  arg.first.c_str(),
191  e.what());
192  }
193  }
194  std::multimap<std::string, std::string> messages;
195  if (!action_skill_mapping_.has_mapping(activity->mapped_name())) {
196  throw Exception(std::string("No mapping for action " + activity->mapped_name()).c_str());
197  }
198  auto mapping{action_skill_mapping_.map_skill(activity->mapped_name(), params, messages)};
199  for (auto m = messages.find("ERROR"); m != messages.end();) {
200  throw InvalidArgumentException("Error occurred while mapping action '%s': %s",
201  activity->mapped_name().c_str(),
202  m->second.c_str());
203  }
204  for (auto m = messages.find("WARNING"); m != messages.end();) {
205  logger_->log_warn(name(),
206  "Warning occurred while mapping action '%s': %s",
207  activity->mapped_name().c_str(),
208  m->second.c_str());
209  }
210  return mapping;
211 }
212 
213 /** Update the status of the activity according to the Skiller status.
214  * @param iface The interface that has changed
215  */
216 void
218 {
219  if (!running_activity_) {
220  return;
221  }
222  SkillerInterface *skiller_if = dynamic_cast<SkillerInterface *>(iface);
223  if (!skiller_if) {
224  return;
225  }
226  skiller_if->read();
227  switch (skiller_if->status()) {
229  running_activity_->update(Transition::Hook::FINISH);
230  running_activity_.reset();
231  break;
233  running_activity_->update(Transition::Hook::FAIL);
234  running_activity_.reset();
235  break;
236  case SkillerInterface::S_RUNNING: running_activity_->update(Transition::Hook::START); break;
237  default: break;
238  }
239 }
240 
241 } // namespace gpp
242 } // namespace fawkes
fawkes::gpp::SkillerActionExecutor::name
const char * name() const
Get the name of the executor; mainly used for logging.
Definition: skiller_action_executor.cpp:176
fawkes::BlackBoard::BBIL_FLAG_DATA
@ BBIL_FLAG_DATA
consider data events
Definition: blackboard.h:88
fawkes::BlackBoard::register_listener
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
fawkes::gpp::InvalidArgumentException
An exception that is thrown if the given arguments do not match the skill's arguments.
Definition: skiller_action_executor.h:39
fawkes::SkillerInterface::S_RUNNING
@ S_RUNNING
The execution is still running.
Definition: SkillerInterface.h:47
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:472
fawkes::gpp::InvalidArgumentException::InvalidArgumentException
InvalidArgumentException(const char *format,...)
Constructor.
Definition: skiller_action_executor.cpp:41
fawkes::SkillerInterface::ReleaseControlMessage
ReleaseControlMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:186
fawkes::BlackBoard::unregister_listener
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
fawkes::BlackBoardInterfaceListener
BlackBoard interface listener.
Definition: interface_listener.h:42
fawkes::BlackBoard
The BlackBoard abstract class.
Definition: blackboard.h:46
fawkes::gpp::SkillerActionExecutor::blackboard_
BlackBoard * blackboard_
The blackboard to use to access the skiller.
Definition: skiller_action_executor.h:59
fawkes::SkillerInterface::S_FINAL
@ S_FINAL
The skill string has been successfully processed.
Definition: SkillerInterface.h:46
fawkes::Configuration
Interface for configuration handling.
Definition: config.h:65
fawkes::Configuration::search
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
fawkes::ActionSkillMapping::has_mapping
bool has_mapping(const std::string &action_name) const
Check if mapping for an action exists.
Definition: map_skill.cpp:125
fawkes::gpp::SkillerActionExecutor::can_execute_activity
bool can_execute_activity(std::shared_ptr< gologpp::Activity > activity) const override
Check if we can execute the given activity.
Definition: skiller_action_executor.cpp:133
fawkes::SkillerInterface::AcquireControlMessage
AcquireControlMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:155
fawkes::SkillerInterface::ExecSkillMessage
ExecSkillMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:84
fawkes::SkillerInterface::StopExecMessage
StopExecMessage Fawkes BlackBoard Interface Message.
Definition: SkillerInterface.h:134
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
fawkes::Logger
Interface for logging.
Definition: logger.h:42
fawkes::SkillerInterface::S_FAILED
@ S_FAILED
The execution failed and cannot succeed anymore.
Definition: SkillerInterface.h:48
fawkes::gpp::SkillerActionExecutor::~SkillerActionExecutor
virtual ~SkillerActionExecutor() override
Destructor.
Definition: skiller_action_executor.cpp:116
fawkes
Fawkes library namespace.
fawkes::gpp::ActionExecutor::running_activity_
std::shared_ptr< gologpp::Activity > running_activity_
A pointer to the currently running activity.
Definition: action_executor.h:43
fawkes::gpp::ActionExecutor::logger_
Logger * logger_
The logger to use for logging messages.
Definition: action_executor.h:44
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
fawkes::Interface
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:79
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:817
fawkes::gpp::SkillerActionExecutor::start
void start(std::shared_ptr< gologpp::Activity > activity) override
Start the given activity.
Definition: skiller_action_executor.cpp:143
fawkes::gpp::SkillerActionExecutor::stop
void stop(std::shared_ptr< gologpp::Grounding< gologpp::Action >> activity) override
Stop the activity if it is currently running.
Definition: skiller_action_executor.cpp:164
fawkes::SkillerInterface
SkillerInterface Fawkes BlackBoard Interface.
Definition: SkillerInterface.h:34
fawkes::ActionSkillMapping::map_skill
std::string map_skill(const std::string &name, const std::map< std::string, std::string > &params, std::multimap< std::string, std::string > &messages) const
Perform mapping.
Definition: map_skill.cpp:139
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::gpp::SkillerActionExecutor::bb_interface_data_changed
virtual void bb_interface_data_changed(Interface *) override
Update the status of the activity according to the Skiller status.
Definition: skiller_action_executor.cpp:217
fawkes::Exception::what
virtual const char * what() const
Get primary string.
Definition: exception.cpp:639
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
fawkes::gpp::SkillerActionExecutor::SkillerActionExecutor
SkillerActionExecutor(Logger *logger, BlackBoard *blackboard, Configuration *config, const std::string &cfg_prefix)
Constructor.
Definition: skiller_action_executor.cpp:71
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
fawkes::BlackBoardInterfaceListener::bbil_add_data_interface
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
Definition: interface_listener.cpp:232
fawkes::gpp::ActionExecutor
Abstract class to execute a Golog++ activity.
Definition: action_executor.h:35
fawkes::Exception::append_nolock_va
void append_nolock_va(const char *format, va_list va)
Append messages without lock by formatted string.
Definition: exception.cpp:449
fawkes::gpp::SkillerActionExecutor::blackboard_owner_
bool blackboard_owner_
True if this executor is owning its blackboard.
Definition: skiller_action_executor.h:60
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36