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 
28 using 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 
42 void
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 
70 void
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 
81 void
83 {
84  server_->start();
85 }
86 
87 void
88 RosSkillerThread::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 
105 void
106 RosSkillerThread::action_goal_cb(SkillerServer::GoalHandle goal)
107 {
108  MutexLocker lock(loop_mutex);
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 
121 void
122 RosSkillerThread::action_cancel_cb(SkillerServer::GoalHandle goal)
123 {
124  MutexLocker lock(loop_mutex);
125  stop();
126  std::string error_msg = "Abort on request";
127  goal.setCanceled(create_result(error_msg), error_msg);
128 }
129 
130 void
131 RosSkillerThread::message_cb(const std_msgs::String::ConstPtr &goal)
132 {
133  MutexLocker lock(loop_mutex);
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 
140 fawkes_msgs::ExecSkillResult
141 RosSkillerThread::create_result(const std::string &errmsg)
142 {
143  fawkes_msgs::ExecSkillResult result;
144  result.errmsg = errmsg;
145  return result;
146 }
147 
148 fawkes_msgs::ExecSkillFeedback
149 RosSkillerThread::create_feedback()
150 {
151  return fawkes_msgs::ExecSkillFeedback();
152 }
153 
154 void
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.