22 #include "skiller_thread.h"
24 #include <core/threading/mutex_locker.h>
25 #include <fawkes_msgs/SkillStatus.h>
26 #include <utils/time/time.h>
37 :
Thread(
"RosSkillerThread",
Thread::OPMODE_WAITFORWAKEUP),
45 exec_request_ =
false;
46 exec_running_ =
false;
56 server_ =
new SkillerServer(**
rosnode,
58 boost::bind(&RosSkillerThread::action_goal_cb,
this, _1),
59 boost::bind(&RosSkillerThread::action_cancel_cb,
this, _1),
63 rosnode->subscribe<std_msgs::String>(
"skiller",
65 boost::bind(&RosSkillerThread::message_cb,
this, _1));
67 pub_status_ =
rosnode->advertise<fawkes_msgs::SkillStatus>(
"skiller_status",
true);
88 RosSkillerThread::stop()
98 std::string error_msg =
"Abort on request";
99 as_goal_.setAborted(create_result(error_msg), error_msg);
102 exec_running_ =
false;
106 RosSkillerThread::action_goal_cb(SkillerServer::GoalHandle goal)
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);
114 goal_ = goal.getGoal()->skillstring;
115 exec_request_ =
true;
122 RosSkillerThread::action_cancel_cb(SkillerServer::GoalHandle goal)
126 std::string error_msg =
"Abort on request";
127 goal.setCanceled(create_result(error_msg), error_msg);
131 RosSkillerThread::message_cb(
const std_msgs::String::ConstPtr &goal)
136 exec_request_ =
true;
140 fawkes_msgs::ExecSkillResult
141 RosSkillerThread::create_result(
const std::string &errmsg)
143 fawkes_msgs::ExecSkillResult result;
144 result.errmsg = errmsg;
148 fawkes_msgs::ExecSkillFeedback
149 RosSkillerThread::create_feedback()
151 return fawkes_msgs::ExecSkillFeedback();
160 if (!exec_running_ && !exec_request_
180 exec_request_ =
false;
189 exec_running_ =
true;
190 exec_msgid_ = msg->
id();
199 }
else if (exec_running_) {
201 as_goal_.publishFeedback(create_feedback());
203 if (skiller_if_->
status() == SkillerInterface::S_INACTIVE
204 || skiller_if_->
msgid() != exec_msgid_) {
208 if (loops_waited_ >= 3) {
211 std::string error_msg =
"Skiller doesn't start";
213 as_goal_.setAborted(create_result(error_msg), error_msg);
214 exec_running_ =
false;
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";
225 if (asprintf(&tmp,
"Failed to execute skill, error: %s", skiller_if_->
error()) != -1) {
229 as_goal_.setAborted(create_result(error_msg), error_msg);
236 fawkes_msgs::SkillStatus msg;
240 msg.error = skiller_if_->
error();
241 msg.status = skiller_if_->
status();
242 pub_status_.publish(msg);