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);
88RosSkillerThread::stop()
98 std::string error_msg =
"Abort on request";
99 as_goal_.setAborted(create_result(error_msg), error_msg);
102 exec_running_ =
false;
106RosSkillerThread::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;
122RosSkillerThread::action_cancel_cb(SkillerServer::GoalHandle goal)
126 std::string error_msg =
"Abort on request";
127 goal.setCanceled(create_result(error_msg), error_msg);
131RosSkillerThread::message_cb(
const std_msgs::String::ConstPtr &goal)
136 exec_request_ =
true;
140fawkes_msgs::ExecSkillResult
141RosSkillerThread::create_result(
const std::string &errmsg)
143 fawkes_msgs::ExecSkillResult result;
144 result.errmsg = errmsg;
148fawkes_msgs::ExecSkillFeedback
149RosSkillerThread::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);
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.
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.
const Time * timestamp() const
Get timestamp of last write.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Uuid serial() const
Get instance serial of interface.
void read()
Read from BlackBoard into local copy.
bool has_writer() const
Check if there is a writer for the interface.
bool refreshed() const
Check if data has been refreshed.
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.
unsigned int id() const
Get message ID.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
void unref()
Decrement reference count and conditionally delete this instance.
void ref()
Increment reference count.
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.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
const char * name() const
Get name of thread.
A class for handling time.
Time & stamp()
Set this time to the current time.
long get_nsec() const
Get nanoseconds.
long get_sec() const
Get seconds.
std::string get_string() const
Get the string representation of the Uuid.
Fawkes library namespace.