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);
char * skill_string() const
Get skill_string value.
virtual void once()
Execute an action exactly once.
unsigned int id() const
Get message ID.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
void unref()
Decrement reference count and conditionally delete this instance.
Fawkes library namespace.
char * skill_string() const
Get skill_string value.
StopExecMessage Fawkes BlackBoard Interface Message.
char * error() const
Get error value.
A class for handling time.
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.
Mutex * loop_mutex
Mutex that is used to protect a call to loop().
Logger * logger
This is the Logger member used to access the logger.
AcquireControlMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
unsigned short serial() const
Get instance serial of interface.
void read()
Read from BlackBoard into local copy.
long get_nsec() const
Get nanoseconds.
void ref()
Increment reference count.
bool has_writer() const
Check if there is a writer for the interface.
SkillStatusEnum status() const
Get status value.
ExecSkillMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
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.
bool changed() const
Check if data has been changed.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
long get_sec() const
Get seconds.
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.
uint32_t msgid() const
Get msgid value.
SkillerInterface Fawkes BlackBoard Interface.
Time & stamp()
Set this time to the current time.
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.
virtual void close(Interface *interface)=0
Close interface.
virtual void finalize()
Finalize the thread.