fawkes_skiller_node.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * fawkes_skiller_node - Skiller interface similar to Fawkes' ros-skiller
3  *
4  * Created: Tue May 31 21:24:09 2016
5  * Copyright 2016 Tim Niemueller [www.niemueller.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 
22 #include <ros/ros.h>
23 #include <ros/callback_queue.h>
24 
25 #include <blackboard/remote.h>
26 #include <interfaces/SkillerInterface.h>
27 #include <utils/time/time.h>
28 
29 #include <actionlib/server/simple_action_server.h>
30 #include <fawkes_msgs/ExecSkillAction.h>
31 #include <fawkes_msgs/ExecSkillGoal.h>
32 #include <fawkes_msgs/ExecSkillActionGoal.h>
33 #include <fawkes_msgs/SkillStatus.h>
34 
35 #include <memory>
36 
37 #include <mutex>
38 
39 #define GET_PRIV_PARAM(P) \
40  { \
41  if (! ros::param::get("~" #P, cfg_ ## P ## _)) { \
42  ROS_ERROR("%s: Failed to retrieve parameter " #P ", aborting", ros::this_node::getName().c_str()); \
43  exit(-1); \
44  } \
45  }
46 
47 using namespace fawkes;
48 
49 typedef actionlib::ActionServer<fawkes_msgs::ExecSkillAction> SkillerServer;
50 
51 
52 class RosSkillerNode
53 {
54  public:
55  RosSkillerNode(ros::NodeHandle *rn, std::shared_ptr<fawkes::BlackBoard> bb)
56  : rosnode(rn), blackboard(bb) {}
57 
58  fawkes_msgs::ExecSkillResult
59  create_result(std::string errmsg)
60  {
61  fawkes_msgs::ExecSkillResult result;
62  result.errmsg = errmsg;
63  return result;
64  }
65 
66 
67  fawkes_msgs::ExecSkillFeedback
68  create_feedback()
69  {
70  return fawkes_msgs::ExecSkillFeedback();
71  }
72 
73  void
74  action_goal_cb(SkillerServer::GoalHandle goal)
75  {
76  ROS_INFO("%s: Received goal to execute %s", ros::this_node::getName().c_str(),
77  goal.getGoal()->skillstring.c_str());
78  std::lock_guard<std::mutex> lock(loop_mutex);
79  if (exec_running_ && exec_as_) {
80  std::string error_msg = "Replaced by new goal";
81  as_goal_.setAborted(create_result(error_msg), error_msg);
82  }
83  if (! blackboard || !blackboard->is_alive()) {
84  std::string error_msg = "Blackboard not connected";
85  goal.setAborted(create_result(error_msg), error_msg);
86  return;
87  }
88  as_goal_ = goal;
89  goal_ = goal.getGoal()->skillstring;
90  exec_request_ = true;
91  exec_as_ = true;
92 
93  ROS_INFO("%s: Accepting goal '%s'", ros::this_node::getName().c_str(), goal_.c_str());
94  goal.setAccepted();
95  }
96 
97  void
98  action_cancel_cb(SkillerServer::GoalHandle goal)
99  {
100  std::lock_guard<std::mutex> lock(loop_mutex);
101  stop();
102  std::string error_msg = "Abort on request";
103  goal.setCanceled(create_result(error_msg), error_msg);
104  }
105 
106  virtual void init()
107  {
108  exec_request_ = false;
109  exec_running_ = false;
110  exec_as_ = false;
111  wait_release_ = 0;
112 
113  try {
114  skiller_if_ = blackboard->open_for_reading<SkillerInterface>("Skiller");
115  } catch (const Exception &e) {
116  ROS_ERROR("%s: Initialization failed, could not open Skiller interface", ros::this_node::getName().c_str());
117  throw;
118  }
119 
120  server_ = new SkillerServer(*rosnode, "skiller",
121  boost::bind(&RosSkillerNode::action_goal_cb, this, _1),
122  boost::bind(&RosSkillerNode::action_cancel_cb, this, _1),
123  /* auto_start */ false);
124 
125  pub_status_ = rosnode->advertise<fawkes_msgs::SkillStatus>("skiller_status", true);
126 
127  server_->start();
128  }
129 
130  virtual void finalize()
131  {
132  try {
133  blackboard->close(skiller_if_);
134  } catch (Exception& e) {
135  ROS_ERROR("%s: Closing interface failed!", ros::this_node::getName().c_str());
136  }
137  pub_status_.shutdown();
138  delete server_;
139  }
140 
141  void
142  stop()
143  {
144  if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
145  ROS_ERROR("%s: Skill abortion requested, but not in control", ros::this_node::getName().c_str());
146  return;
147  }
148 
149  if (skiller_if_->has_writer())
150  skiller_if_->msgq_enqueue(new SkillerInterface::StopExecMessage());
151  if (exec_as_) {
152  std::string error_msg = "Abort on request";
153  as_goal_.setAborted(create_result(error_msg), error_msg);
154  }
155  skiller_if_->msgq_enqueue(new SkillerInterface::ReleaseControlMessage());
156  exec_running_ = false;
157  }
158 
159  void
160  loop()
161  {
162  std::lock_guard<std::mutex> lock(loop_mutex);
163  skiller_if_->read();
164 
165  // currently idle, release skiller control
166  if (!exec_running_ && !exec_request_ && wait_release_ == 0 &&
167  skiller_if_->exclusive_controller() == skiller_if_->serial())
168  {
169  ROS_INFO("%s: No skill running and no skill requested, releasing control",
170  ros::this_node::getName().c_str());
171  skiller_if_->msgq_enqueue(new SkillerInterface::ReleaseControlMessage());
172  wait_release_ = 1;
173  return;
174  }
175 
176  if (wait_release_ > 0 &&
177  skiller_if_->exclusive_controller() == skiller_if_->serial())
178  {
179  if (++wait_release_ % 50 == 0) {
180  ROS_WARN("%s: waiting for exclusive control release",
181  ros::this_node::getName().c_str());
182  }
183  ros::WallDuration(0.1).sleep();
184  if (wait_release_ > 30) {
185  // Reset, will triggering another release
186  wait_release_ = 0;
187  }
188  return;
189  }
190 
191  if (exec_request_) {
192  if (!skiller_if_->has_writer()) {
193  std::string error_msg("no writer for skiller, cannot execute");
194  ROS_WARN("%s: %s", ros::this_node::getName().c_str(), error_msg.c_str());
195  stop();
196  as_goal_.setAborted(create_result(error_msg), error_msg);
197  return;
198  }
199 
200  if (skiller_if_->exclusive_controller() != skiller_if_->serial()) {
201  // we need the skiller control, acquire it first
202  ROS_INFO("%s: Skill execution requested, acquiring control", ros::this_node::getName().c_str());
203  skiller_if_->msgq_enqueue(new SkillerInterface::AcquireControlMessage());
204  return;
205  }
206  exec_request_ = false;
207 
208  SkillerInterface::ExecSkillMessage *msg =
209  new SkillerInterface::ExecSkillMessage(goal_.c_str());
210  msg->ref();
211 
212  ROS_INFO("%s: Creating goal '%s'", ros::this_node::getName().c_str(), goal_.c_str());
213 
214  try {
215  skiller_if_->msgq_enqueue(msg);
216  exec_running_ = true;
217  exec_msgid_ = msg->id();
218  exec_skill_string_ = msg->skill_string();
219  loops_waited_ = 0;
220  } catch (Exception &e) {
221  ROS_WARN("%s: Failed to execute skill, exception %s", ros::this_node::getName().c_str(), e.what_no_backtrace());
222  }
223  msg->unref();
224 
225  } else if (exec_running_) {
226  if (exec_as_) as_goal_.publishFeedback(create_feedback());
227 
228  if (skiller_if_->status() == SkillerInterface::S_INACTIVE ||
229  skiller_if_->msgid() != exec_msgid_)
230  {
231  // wait three loops, maybe the skiller will start
232  //ROS_INFO("Should be executing skill, but skiller is inactive or processing other message");
233  ++loops_waited_;
234  if (loops_waited_ >= 5) {
235  // give up and abort
236  ROS_WARN("%s: Skiller doesn't start, aborting", ros::this_node::getName().c_str());
237  std::string error_msg = "Skiller doesn't start";
238  if (exec_as_) as_goal_.setAborted(create_result(error_msg), error_msg);
239  exec_running_ = false;
240  }
241  }
242  else if (skiller_if_->status() != SkillerInterface::S_RUNNING) {
243  exec_running_ = false;
244  if (exec_as_ && exec_skill_string_ == skiller_if_->skill_string()) {
245  if (skiller_if_->status() == SkillerInterface::S_FINAL) {
246  std::string error_msg = "Skill executed";
247  as_goal_.setSucceeded(create_result(error_msg), error_msg);
248  }
249  else if (skiller_if_->status() == SkillerInterface::S_FAILED){
250  std::string error_msg = "Failed to execute skill";
251  char * tmp;
252  if (asprintf(&tmp, "Failed to execute skill, error: %s", skiller_if_->error()) != -1) {
253  error_msg = tmp;
254  free(tmp);
255  }
256  as_goal_.setAborted(create_result(error_msg), error_msg);
257  }
258  else {
259  ROS_WARN("%s: Don't know what happened, but not running", ros::this_node::getName().c_str());
260  }
261  }
262  }
263  }
264 
265  if (skiller_if_->changed()) {
266  fawkes_msgs::SkillStatus msg;
267  const Time *time = skiller_if_->timestamp();
268  msg.stamp = ros::Time(time->get_sec(), time->get_nsec());
269  msg.skill_string = skiller_if_->skill_string();
270  msg.error = skiller_if_->error();
271  msg.status = skiller_if_->status();
272  pub_status_.publish(msg);
273  }
274  }
275 
276  private:
277  ros::NodeHandle *rosnode;
278  std::shared_ptr<fawkes::BlackBoard> blackboard;
279 
280  std::mutex loop_mutex;
281 
282  fawkes::SkillerInterface *skiller_if_;
283 
284  SkillerServer *server_;
285  ros::Publisher pub_status_;
286 
287  SkillerServer::GoalHandle as_goal_;
288  std::string goal_;
289 
290  bool exec_as_;
291  bool exec_request_;
292  bool exec_running_;
293  unsigned int exec_msgid_;
294  std::string exec_skill_string_;
295  unsigned int loops_waited_;
296 
297  unsigned int wait_release_;
298 };
299 
300 
301 int
302 main(int argc, char **argv)
303 {
304  ros::init(argc, argv, "fawkes_skiller_forward");
305 
306  ros::NodeHandle n;
307 
308  std::shared_ptr<RosSkillerNode> ros_skiller_node;
309  std::string cfg_fawkes_host_;
310  int cfg_fawkes_port_;
311  std::shared_ptr<fawkes::BlackBoard> blackboard_;
312 
313  GET_PRIV_PARAM(fawkes_host);
314  GET_PRIV_PARAM(fawkes_port);
315 
316  while (ros::ok()) {
317  if (!blackboard_) {
318  ROS_WARN_THROTTLE(30, "%s: Not connected, retrying", ros::this_node::getName().c_str());
319  try {
320  blackboard_ =
321  std::make_shared<fawkes::RemoteBlackBoard>(cfg_fawkes_host_.c_str(), cfg_fawkes_port_);
322  ros_skiller_node = std::make_shared<RosSkillerNode>(&n, blackboard_);
323  ros_skiller_node->init();
324  ROS_INFO("%s: Blackboard connected and initialized", ros::this_node::getName().c_str());
325  } catch (fawkes::Exception &e) {
326  ROS_WARN_THROTTLE(10, "%s: Initialization failed, retrying", ros::this_node::getName().c_str());
327  if (ros_skiller_node) {
328  ros_skiller_node->finalize();
329  }
330  ros_skiller_node.reset();
331  blackboard_.reset();
332  }
333  } else if (! blackboard_->is_alive()) {
334  ROS_WARN_THROTTLE(30, "%s: blackboard connection lost, retrying", ros::this_node::getName().c_str());
335  if (blackboard_->try_aliveness_restore()) {
336  ROS_INFO("%s: Blackboard re-connected", ros::this_node::getName().c_str());
337  }
338  } else {
339  ros_skiller_node->loop();
340  }
341 
342  ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
343  }
344 
345  if (ros_skiller_node) ros_skiller_node->finalize();
346 
347  return 0;
348 }
RosSkillerNode::create_feedback
fawkes_msgs::ExecSkillFeedback create_feedback()
Definition: fawkes_skiller_node.cpp:68
RosSkillerNode::action_cancel_cb
void action_cancel_cb(SkillerServer::GoalHandle goal)
Definition: fawkes_skiller_node.cpp:98
ros::NodeHandle
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(rate,...)
main
int main(int argc, char **argv)
Definition: fawkes_skiller_node.cpp:302
RosSkillerNode::skiller_if_
fawkes::SkillerInterface * skiller_if_
Definition: fawkes_skiller_node.cpp:282
RosSkillerNode::blackboard
std::shared_ptr< fawkes::BlackBoard > blackboard
Definition: fawkes_skiller_node.cpp:278
RosSkillerNode::action_goal_cb
void action_goal_cb(SkillerServer::GoalHandle goal)
Definition: fawkes_skiller_node.cpp:74
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
RosSkillerNode::exec_skill_string_
std::string exec_skill_string_
Definition: fawkes_skiller_node.cpp:294
actionlib::ServerGoalHandle::getGoal
boost::shared_ptr< const Goal > getGoal() const
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
RosSkillerNode::as_goal_
SkillerServer::GoalHandle as_goal_
Definition: fawkes_skiller_node.cpp:287
ROS_WARN
#define ROS_WARN(...)
actionlib::ServerGoalHandle::setCanceled
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
ros::getGlobalCallbackQueue
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
actionlib::ServerGoalHandle::setAccepted
void setAccepted(const std::string &text=std::string(""))
ros::WallDuration::sleep
bool sleep() const
RosSkillerNode::wait_release_
unsigned int wait_release_
Definition: fawkes_skiller_node.cpp:297
ROS_INFO
#define ROS_INFO(...)
RosSkillerNode::rosnode
ros::NodeHandle * rosnode
Definition: fawkes_skiller_node.cpp:277
ros::ok
ROSCPP_DECL bool ok()
RosSkillerNode::server_
SkillerServer * server_
Definition: fawkes_skiller_node.cpp:284
RosSkillerNode::exec_msgid_
unsigned int exec_msgid_
Definition: fawkes_skiller_node.cpp:293
RosSkillerNode::RosSkillerNode
RosSkillerNode(ros::NodeHandle *rn, std::shared_ptr< fawkes::BlackBoard > bb)
Definition: fawkes_skiller_node.cpp:55
RosSkillerNode::create_result
fawkes_msgs::ExecSkillResult create_result(std::string errmsg)
Definition: fawkes_skiller_node.cpp:59
actionlib::ServerGoalHandle::setAborted
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
GET_PRIV_PARAM
#define GET_PRIV_PARAM(P)
Definition: fawkes_skiller_node.cpp:39
SkillerServer
actionlib::ActionServer< fawkes_msgs::ExecSkillAction > SkillerServer
Definition: fawkes_skiller_node.cpp:49
RosSkillerNode::pub_status_
ros::Publisher pub_status_
Definition: fawkes_skiller_node.cpp:285
RosSkillerNode::finalize
virtual void finalize()
Definition: fawkes_skiller_node.cpp:130
ROS_ERROR
#define ROS_ERROR(...)
callback_queue.h
RosSkillerNode::loops_waited_
unsigned int loops_waited_
Definition: fawkes_skiller_node.cpp:295
simple_action_server.h

rcll_fawkes_sim
Author(s): Tim Niemueller
autogenerated on Mon Jun 10 2019 14:31:10