Fawkes API Fawkes Development Version
skiller_thread.cpp
1
2/***************************************************************************
3 * skiller_thread.cpp - ROS Action Server to receive skiller commands from ROS
4 *
5 * Created: Fri Jun 27 12:02:42 2014
6 * Copyright 2014 Till Hofmann
7 ****************************************************************************/
8
9/* This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU Library General Public License for more details.
18 *
19 * Read the full text in the LICENSE.GPL file in the doc directory.
20 */
21
22#include "skiller_thread.h"
23
24#include <core/threading/mutex_locker.h>
25#include <fawkes_msgs/SkillStatus.h>
26#include <utils/time/time.h>
27
28using namespace fawkes;
29
30/** @class RosSkillerThread "skiller_thread.h"
31 * Accept skiller commands from ROS.
32 * @author Till Hofmann
33 */
34
35/** Contructor. */
37: Thread("RosSkillerThread", Thread::OPMODE_WAITFORWAKEUP),
39{
40}
41
42void
44{
45 exec_request_ = false;
46 exec_running_ = false;
47 exec_as_ = false;
48
49 try {
50 skiller_if_ = blackboard->open_for_reading<SkillerInterface>("Skiller");
51 } catch (const Exception &e) {
52 logger->log_error(name(), "Initialization failed, could not open Skiller interface");
53 throw;
54 }
55
56 server_ = new SkillerServer(**rosnode,
57 "skiller",
58 boost::bind(&RosSkillerThread::action_goal_cb, this, _1),
59 boost::bind(&RosSkillerThread::action_cancel_cb, this, _1),
60 /* auto_start */ false);
61
62 sub_cmd_ =
63 rosnode->subscribe<std_msgs::String>("skiller",
64 1,
65 boost::bind(&RosSkillerThread::message_cb, this, _1));
66
67 pub_status_ = rosnode->advertise<fawkes_msgs::SkillStatus>("skiller_status", true);
68}
69
70void
72{
73 try {
74 blackboard->close(skiller_if_);
75 } catch (Exception &e) {
76 logger->log_error(name(), "Closing interface failed!");
77 }
78 delete server_;
79}
80
81void
83{
84 server_->start();
85}
86
87void
88RosSkillerThread::stop()
89{
90 if (skiller_if_->exclusive_controller() != skiller_if_->serial().get_string()) {
91 logger->log_warn(name(), "Skill abortion requested, but currently not in control");
92 return;
93 }
94
95 if (skiller_if_->has_writer())
97 if (exec_as_) {
98 std::string error_msg = "Abort on request";
99 as_goal_.setAborted(create_result(error_msg), error_msg);
100 }
102 exec_running_ = false;
103}
104
105void
106RosSkillerThread::action_goal_cb(SkillerServer::GoalHandle goal)
107{
109 if (exec_running_ && exec_as_) {
110 std::string error_msg = "Replaced by new goal";
111 as_goal_.setAborted(create_result(error_msg), error_msg);
112 }
113 as_goal_ = goal;
114 goal_ = goal.getGoal()->skillstring;
115 exec_request_ = true;
116 exec_as_ = true;
117
118 goal.setAccepted();
119}
120
121void
122RosSkillerThread::action_cancel_cb(SkillerServer::GoalHandle goal)
123{
125 stop();
126 std::string error_msg = "Abort on request";
127 goal.setCanceled(create_result(error_msg), error_msg);
128}
129
130void
131RosSkillerThread::message_cb(const std_msgs::String::ConstPtr &goal)
132{
134 logger->log_info(name(), "Received new goal: '%s'", goal->data.c_str());
135 goal_ = goal->data;
136 exec_request_ = true;
137 exec_as_ = false;
138}
139
140fawkes_msgs::ExecSkillResult
141RosSkillerThread::create_result(const std::string &errmsg)
142{
143 fawkes_msgs::ExecSkillResult result;
144 result.errmsg = errmsg;
145 return result;
146}
147
148fawkes_msgs::ExecSkillFeedback
149RosSkillerThread::create_feedback()
150{
151 return fawkes_msgs::ExecSkillFeedback();
152}
153
154void
156{
157 skiller_if_->read();
158
159 // currently idle, release skiller control
160 if (!exec_running_ && !exec_request_
161 && skiller_if_->exclusive_controller() == skiller_if_->serial().get_string()) {
162 logger->log_debug(name(), "No skill running and no skill requested, releasing control");
164 return;
165 }
166
167 if (exec_request_) {
168 if (!skiller_if_->has_writer()) {
169 logger->log_warn(name(), "no writer for skiller, cannot execute skill");
170 stop();
171 return;
172 }
173
174 if (skiller_if_->exclusive_controller() != skiller_if_->serial().get_string()) {
175 // we need the skiller control, acquire it first
176 logger->log_debug(name(), "Skill execution requested, but currently not in control");
178 return;
179 }
180 exec_request_ = false;
181
183 msg->ref();
184
185 logger->log_debug(name(), "Creating goal '%s'", goal_.c_str());
186
187 try {
188 skiller_if_->msgq_enqueue(msg);
189 exec_running_ = true;
190 exec_msgid_ = msg->id();
191 exec_skill_string_ = msg->skill_string();
192 loops_waited_ = 0;
193 } catch (Exception &e) {
194 logger->log_warn(name(), "Failed to execute skill, exception follows");
195 logger->log_warn(name(), e);
196 }
197 msg->unref();
198
199 } else if (exec_running_) {
200 if (exec_as_)
201 as_goal_.publishFeedback(create_feedback());
202
203 if (skiller_if_->status() == SkillerInterface::S_INACTIVE
204 || skiller_if_->msgid() != exec_msgid_) {
205 // wait three loops, maybe the skiller will start
206 logger->log_debug(name(), "Should be executing skill, but skiller is inactive");
207 ++loops_waited_;
208 if (loops_waited_ >= 3) {
209 // give up and abort
210 logger->log_warn(name(), "Skiller doesn't start, aborting");
211 std::string error_msg = "Skiller doesn't start";
212 if (exec_as_)
213 as_goal_.setAborted(create_result(error_msg), error_msg);
214 exec_running_ = false;
215 }
216 } else if (skiller_if_->status() != SkillerInterface::S_RUNNING) {
217 exec_running_ = false;
218 if (exec_as_ && exec_skill_string_ == skiller_if_->skill_string()) {
219 if (skiller_if_->status() == SkillerInterface::S_FINAL) {
220 std::string error_msg = "Skill executed";
221 as_goal_.setSucceeded(create_result(error_msg), error_msg);
222 } else if (skiller_if_->status() == SkillerInterface::S_FAILED) {
223 std::string error_msg = "Failed to execute skill";
224 char * tmp;
225 if (asprintf(&tmp, "Failed to execute skill, error: %s", skiller_if_->error()) != -1) {
226 error_msg = tmp;
227 free(tmp);
228 }
229 as_goal_.setAborted(create_result(error_msg), error_msg);
230 }
231 }
232 }
233 }
234
235 if (skiller_if_->refreshed()) {
236 fawkes_msgs::SkillStatus msg;
237 const Time * time = skiller_if_->timestamp();
238 msg.stamp = ros::Time(time->get_sec(), time->get_nsec());
239 msg.skill_string = skiller_if_->skill_string();
240 msg.error = skiller_if_->error();
241 msg.status = skiller_if_->status();
242 pub_status_.publish(msg);
243 }
244}
virtual void once()
Execute an action exactly once.
virtual void finalize()
Finalize the thread.
RosSkillerThread()
Contructor.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:714
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
Uuid serial() const
Get instance serial of interface.
Definition: interface.cpp:695
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
bool refreshed() const
Check if data has been refreshed.
Definition: interface.cpp:811
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.
Definition: logging.h:41
unsigned int id() const
Get message ID.
Definition: message.cpp:181
Mutex locking helper.
Definition: mutex_locker.h:34
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:95
void ref()
Increment reference count.
Definition: refcount.cpp:67
AcquireControlMessage Fawkes BlackBoard Interface Message.
ExecSkillMessage Fawkes BlackBoard Interface Message.
char * skill_string() const
Get skill_string value.
ReleaseControlMessage Fawkes BlackBoard Interface Message.
StopExecMessage Fawkes BlackBoard Interface Message.
SkillerInterface Fawkes BlackBoard Interface.
char * error() const
Get error value.
SkillStatusEnum status() const
Get status value.
uint32_t msgid() const
Get msgid value.
char * skill_string() const
Get skill_string value.
char * exclusive_controller() const
Get exclusive_controller value.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:152
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Time & stamp()
Set this time to the current time.
Definition: time.cpp:704
long get_nsec() const
Get nanoseconds.
Definition: time.h:132
long get_sec() const
Get seconds.
Definition: time.h:117
std::string get_string() const
Get the string representation of the Uuid.
Definition: uuid.cpp:107
Fawkes library namespace.