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()) {
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()) {
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()) {
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_->changed()) {
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 }
fawkes::RefCount::unref
void unref()
Decrement reference count and conditionally delete this instance.
Definition: refcount.cpp:101
fawkes::Time::get_sec
long get_sec() const
Definition: time.h:123
fawkes::Interface::timestamp
const Time * timestamp() const
Get timestamp of last write.
Definition: interface.cpp:707
fawkes::Interface::read
void read()
Read from BlackBoard into local copy.
Definition: interface.cpp:477
fawkes::SkillerInterface::ReleaseControlMessage
Definition: SkillerInterface.h:197
fawkes::Time::get_nsec
long get_nsec() const
Definition: time.h:138
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:47
fawkes::Logger::log_info
virtual void log_info(const char *component, const char *format,...)=0
fawkes::MutexLocker
Definition: mutex_locker.h:39
RosSkillerThread::once
virtual void once()
Execute an action exactly once.
Definition: skiller_thread.cpp:82
fawkes::BlockedTimingAspect
Definition: blocked_timing.h:56
RosSkillerThread::finalize
virtual void finalize()
Finalize the thread.
Definition: skiller_thread.cpp:71
fawkes::Thread::name
const char * name() const
Definition: thread.h:100
RosSkillerThread::init
virtual void init()
Initialize the thread.
Definition: skiller_thread.cpp:43
fawkes::SkillerInterface::AcquireControlMessage
Definition: SkillerInterface.h:166
fawkes::SkillerInterface::ExecSkillMessage
Definition: SkillerInterface.h:95
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:53
fawkes::SkillerInterface::StopExecMessage
Definition: SkillerInterface.h:145
fawkes::RefCount::ref
void ref()
Increment reference count.
Definition: refcount.cpp:73
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
RosSkillerThread::RosSkillerThread
RosSkillerThread()
Contructor.
Definition: skiller_thread.cpp:36
fawkes
fawkes::Interface::changed
bool changed() const
Check if data has been changed.
Definition: interface.cpp:782
fawkes::SkillerInterface::status
SkillStatusEnum status() const
Get status value.
Definition: SkillerInterface.cpp:258
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
fawkes::Interface::has_writer
bool has_writer() const
Check if there is a writer for the interface.
Definition: interface.cpp:819
fawkes::SkillerInterface::msgid
uint32_t msgid() const
Get msgid value.
Definition: SkillerInterface.cpp:222
fawkes::SkillerInterface
Definition: SkillerInterface.h:39
RosSkillerThread::loop
virtual void loop()
Code to execute in the thread.
Definition: skiller_thread.cpp:155
fawkes::SkillerInterface::skill_string
char * skill_string() const
Get skill_string value.
Definition: SkillerInterface.cpp:109
fawkes::SkillerInterface::ExecSkillMessage::skill_string
char * skill_string() const
Get skill_string value.
Definition: SkillerInterface.cpp:398
fawkes::Time
Definition: time.h:98
fawkes::SkillerInterface::error
char * error() const
Get error value.
Definition: SkillerInterface.cpp:146
fawkes::Thread
Definition: thread.h:45
fawkes::Interface::serial
unsigned short serial() const
Get instance serial of interface.
Definition: interface.cpp:688
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:49
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:186
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Time::stamp
Time & stamp()
Set this time to the current time.
Definition: time.cpp:711
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:884
fawkes::SkillerInterface::exclusive_controller
uint32_t exclusive_controller() const
Get exclusive_controller value.
Definition: SkillerInterface.cpp:184
fawkes::Thread::loop_mutex
Mutex * loop_mutex
Definition: thread.h:152
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
fawkes::Exception
Definition: exception.h:41