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 <utils/time/time.h>
26 
27 #include <fawkes_msgs/SkillStatus.h>
28 
29 using namespace fawkes;
30 
31 /** @class RosSkillerThread "skiller_thread.h"
32  * Accept skiller commands from ROS.
33  * @author Till Hofmann
34  */
35 
36 /** Contructor. */
38  : Thread("RosSkillerThread", Thread::OPMODE_WAITFORWAKEUP),
40 {
41 }
42 
43 void
45 {
46  exec_request_ = false;
47  exec_running_ = false;
48  exec_as_ = false;
49 
50  try {
51  skiller_if_ = blackboard->open_for_reading<SkillerInterface>("Skiller");
52  } catch (const Exception &e) {
53  logger->log_error(name(), "Initialization failed, could not open Skiller interface");
54  throw;
55  }
56 
57 
58  server_ = new SkillerServer(**rosnode, "skiller",
59  boost::bind(&RosSkillerThread::action_goal_cb, this, _1),
60  boost::bind(&RosSkillerThread::action_cancel_cb, this, _1),
61  /* auto_start */ false);
62 
63  sub_cmd_ = rosnode->subscribe<std_msgs::String>
64  ("skiller", 1, boost::bind(&RosSkillerThread::message_cb, this, _1));
65 
66  pub_status_ = rosnode->advertise<fawkes_msgs::SkillStatus>("skiller_status", true);
67 }
68 
69 void
71 {
72  try {
73  blackboard->close(skiller_if_);
74  } catch (Exception& e) {
75  logger->log_error(name(), "Closing interface failed!");
76  }
77  delete server_;
78 }
79 
80 
81 void
83 {
84  server_->start();
85 }
86 
87 
88 void
89 RosSkillerThread::stop()
90 {
91  if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
92  logger->log_warn(name(), "Skill abortion requested, but currently not in control");
93  return;
94  }
95 
96  if (skiller_if_->has_writer())
98  if (exec_as_) {
99  std::string error_msg = "Abort on request";
100  as_goal_.setAborted(create_result(error_msg), error_msg);
101  }
103  exec_running_ = false;
104 }
105 
106 void
107 RosSkillerThread::action_goal_cb(SkillerServer::GoalHandle goal)
108 {
109  MutexLocker lock(loop_mutex);
110  if (exec_running_ && exec_as_) {
111  std::string error_msg = "Replaced by new goal";
112  as_goal_.setAborted(create_result(error_msg), error_msg);
113  }
114  as_goal_ = goal;
115  goal_ = goal.getGoal()->skillstring;
116  exec_request_ = true;
117  exec_as_ = true;
118 
119  goal.setAccepted();
120 }
121 
122 void
123 RosSkillerThread::action_cancel_cb(SkillerServer::GoalHandle goal)
124 {
125  MutexLocker lock(loop_mutex);
126  stop();
127  std::string error_msg = "Abort on request";
128  goal.setCanceled(create_result(error_msg), error_msg);
129 }
130 
131 void
132 RosSkillerThread::message_cb(const std_msgs::String::ConstPtr & goal)
133 {
134  MutexLocker lock(loop_mutex);
135  logger->log_info(name(), "Received new goal: '%s'", goal->data.c_str());
136  goal_ = goal->data;
137  exec_request_ = true;
138  exec_as_ = false;
139 }
140 
141 fawkes_msgs::ExecSkillResult
142 RosSkillerThread::create_result(std::string errmsg)
143 {
144  fawkes_msgs::ExecSkillResult result;
145  result.errmsg = errmsg;
146  return result;
147 }
148 
149 
150 fawkes_msgs::ExecSkillFeedback
151 RosSkillerThread::create_feedback()
152 {
153  return fawkes_msgs::ExecSkillFeedback();
154 }
155 
156 void
158 {
159  skiller_if_->read();
160 
161  // currently idle, release skiller control
162  if (!exec_running_ && !exec_request_
163  && skiller_if_->exclusive_controller() == skiller_if_->serial()) {
164  logger->log_debug(name(), "No skill running and no skill requested, releasing control");
166  return;
167  }
168 
169  if (exec_request_) {
170  if (!skiller_if_->has_writer()) {
171  logger->log_warn(name(), "no writer for skiller, cannot execute skill");
172  stop();
173  return;
174  }
175 
176  if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
177  // we need the skiller control, acquire it first
178  logger->log_debug(name(), "Skill execution requested, but currently not in control");
180  return;
181  }
182  exec_request_ = false;
183 
185  new SkillerInterface::ExecSkillMessage(goal_.c_str());
186  msg->ref();
187 
188  logger->log_debug(name(), "Creating goal '%s'", goal_.c_str());
189 
190  try {
191  skiller_if_->msgq_enqueue(msg);
192  exec_running_ = true;
193  exec_msgid_ = msg->id();
194  exec_skill_string_ = msg->skill_string();
195  loops_waited_ = 0;
196  } catch (Exception &e) {
197  logger->log_warn(name(), "Failed to execute skill, exception follows");
198  logger->log_warn(name(), e);
199  }
200  msg->unref();
201 
202  } else if (exec_running_) {
203  if (exec_as_) as_goal_.publishFeedback(create_feedback());
204 
205  if (skiller_if_->status() == SkillerInterface::S_INACTIVE ||
206  skiller_if_->msgid() != exec_msgid_)
207  {
208  // wait three loops, maybe the skiller will start
209  logger->log_debug(name(), "Should be executing skill, but skiller is inactive");
210  ++loops_waited_;
211  if (loops_waited_ >= 3) {
212  // give up and abort
213  logger->log_warn(name(), "Skiller doesn't start, aborting");
214  std::string error_msg = "Skiller doesn't start";
215  if (exec_as_) as_goal_.setAborted(create_result(error_msg), error_msg);
216  exec_running_ = false;
217  }
218  }
219  else if (skiller_if_->status() != SkillerInterface::S_RUNNING) {
220  exec_running_ = false;
221  if (exec_as_ && exec_skill_string_ == skiller_if_->skill_string()) {
222  if (skiller_if_->status() == SkillerInterface::S_FINAL) {
223  std::string error_msg = "Skill executed";
224  as_goal_.setSucceeded(create_result(error_msg), error_msg);
225  }
226  else if (skiller_if_->status() == SkillerInterface::S_FAILED){
227  std::string error_msg = "Failed to execute skill";
228  char * tmp;
229  if (asprintf(&tmp, "Failed to execute skill, error: %s", skiller_if_->error()) != -1) {
230  error_msg = tmp;
231  free(tmp);
232  }
233  as_goal_.setAborted(create_result(error_msg), error_msg);
234  }
235  }
236  }
237  }
238 
239  if (skiller_if_->changed()) {
240  fawkes_msgs::SkillStatus msg;
241  const Time *time = skiller_if_->timestamp();
242  msg.stamp = ros::Time(time->get_sec(), time->get_nsec());
243  msg.skill_string = skiller_if_->skill_string();
244  msg.error = skiller_if_->error();
245  msg.status = skiller_if_->status();
246  pub_status_.publish(msg);
247  }
248 }
char * skill_string() const
Get skill_string value.
virtual void once()
Execute an action exactly once.
unsigned int id() const
Get message ID.
Definition: message.cpp:197
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:99
Fawkes library namespace.
Mutex locking helper.
Definition: mutex_locker.h:33
char * skill_string() const
Get skill_string value.
StopExecMessage Fawkes BlackBoard Interface Message.
char * error() const
Get error value.
A class for handling time.
Definition: time.h:91
ReleaseControlMessage Fawkes BlackBoard Interface Message.
virtual void init()
Initialize the thread.
uint32_t exclusive_controller() const
Get exclusive_controller value.
Thread class encapsulation of pthreads.
Definition: thread.h:42
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Definition: thread.h:139
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
AcquireControlMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
unsigned short serial() const
Get instance serial of interface.
Definition: interface.cpp:697
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
long get_nsec() const
Get nanoseconds.
Definition: time.h:113
void ref()
Increment reference count.
Definition: refcount.cpp:70
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:834
SkillStatusEnum status() const
Get status value.
ExecSkillMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:95
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.
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:718
bool changed() const
Check if data has been changed.
Definition: interface.cpp:796
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
long get_sec() const
Get seconds.
Definition: time.h:110
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
uint32_t msgid() const
Get msgid value.
SkillerInterface Fawkes BlackBoard Interface.
Time & stamp()
Set this time to the current time.
Definition: time.cpp:783
virtual void loop()
Code to execute in the thread.
RosSkillerThread()
Contructor.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.
virtual void finalize()
Finalize the thread.