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
29namespace fawkes {
30namespace gpp {
31
32using 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
99void
100SkillerActionExecutor::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 */
132bool
133SkillerActionExecutor::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 */
142void
143SkillerActionExecutor::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 */
163void
164SkillerActionExecutor::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 */
175const char *
177{
178 return "SkillerActionExecutor";
179}
180
181std::string
182SkillerActionExecutor::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();) {
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 */
216void
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
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
bool has_mapping(const std::string &action_name) const
Check if mapping for an action exists.
Definition: map_skill.cpp:125
BlackBoard interface listener.
void bbil_add_data_interface(Interface *interface)
Add an interface to the data modification watch list.
The BlackBoard abstract class.
Definition: blackboard.h:46
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
@ BBIL_FLAG_DATA
consider data events
Definition: blackboard.h:88
virtual void unregister_listener(BlackBoardInterfaceListener *listener)
Unregister BB interface listener.
Definition: blackboard.cpp:212
virtual void register_listener(BlackBoardInterfaceListener *listener, ListenerRegisterFlag flag=BBIL_FLAG_ALL)
Register BB event listener.
Definition: blackboard.cpp:185
virtual void close(Interface *interface)=0
Close interface.
virtual std::string get_as_string() const =0
Get value as string.
Interface for configuration handling.
Definition: config.h:68
virtual ValueIterator * search(const char *path)=0
Iterator with search results.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what() const noexcept
Get primary string.
Definition: exception.cpp:639
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
void append_nolock_va(const char *format, va_list va) noexcept
Append messages without lock by formatted string.
Definition: exception.cpp:449
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:479
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:848
Interface for logging.
Definition: logger.h:42
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.
AcquireControlMessage Fawkes BlackBoard Interface Message.
ExecSkillMessage Fawkes BlackBoard Interface Message.
ReleaseControlMessage Fawkes BlackBoard Interface Message.
StopExecMessage Fawkes BlackBoard Interface Message.
SkillerInterface Fawkes BlackBoard Interface.
@ S_RUNNING
The execution is still running.
@ S_FAILED
The execution failed and cannot succeed anymore.
@ S_FINAL
The skill string has been successfully processed.
Abstract class to execute a Golog++ activity.
std::shared_ptr< gologpp::Activity > running_activity_
A pointer to the currently running activity.
Logger * logger_
The logger to use for logging messages.
An exception that is thrown if the given arguments do not match the skill's arguments.
InvalidArgumentException(const char *format,...)
Constructor.
const char * name() const
Get the name of the executor; mainly used for logging.
void stop(std::shared_ptr< gologpp::Grounding< gologpp::Action > > activity) override
Stop the activity if it is currently running.
SkillerActionExecutor(Logger *logger, BlackBoard *blackboard, Configuration *config, const std::string &cfg_prefix)
Constructor.
void start(std::shared_ptr< gologpp::Activity > activity) override
Start the given activity.
virtual void bb_interface_data_refreshed(Interface *) noexcept override
Update the status of the activity according to the Skiller status.
bool blackboard_owner_
True if this executor is owning its blackboard.
virtual ~SkillerActionExecutor() override
Destructor.
BlackBoard * blackboard_
The blackboard to use to access the skiller.
bool can_execute_activity(std::shared_ptr< gologpp::Activity > activity) const override
Check if we can execute the given activity.
Fawkes library namespace.