22 #include "skiller_thread.h" 24 #include <core/threading/mutex_locker.h> 25 #include <utils/time/time.h> 27 #include <fawkes_msgs/SkillStatus.h> 38 :
Thread(
"RosSkillerThread",
Thread::OPMODE_WAITFORWAKEUP),
46 exec_request_ =
false;
47 exec_running_ =
false;
58 server_ =
new SkillerServer(**
rosnode,
"skiller",
59 boost::bind(&RosSkillerThread::action_goal_cb,
this, _1),
60 boost::bind(&RosSkillerThread::action_cancel_cb,
this, _1),
63 sub_cmd_ =
rosnode->subscribe<std_msgs::String>
64 (
"skiller", 1, boost::bind(&RosSkillerThread::message_cb,
this, _1));
66 pub_status_ =
rosnode->advertise<fawkes_msgs::SkillStatus>(
"skiller_status",
true);
89 RosSkillerThread::stop()
99 std::string error_msg =
"Abort on request";
100 as_goal_.setAborted(create_result(error_msg), error_msg);
103 exec_running_ =
false;
107 RosSkillerThread::action_goal_cb(SkillerServer::GoalHandle goal)
110 if (exec_running_ && exec_as_) {
111 std::string error_msg =
"Replaced by new goal";
112 as_goal_.setAborted(create_result(error_msg), error_msg);
115 goal_ = goal.getGoal()->skillstring;
116 exec_request_ =
true;
123 RosSkillerThread::action_cancel_cb(SkillerServer::GoalHandle goal)
127 std::string error_msg =
"Abort on request";
128 goal.setCanceled(create_result(error_msg), error_msg);
132 RosSkillerThread::message_cb(
const std_msgs::String::ConstPtr & goal)
137 exec_request_ =
true;
141 fawkes_msgs::ExecSkillResult
142 RosSkillerThread::create_result(std::string errmsg)
144 fawkes_msgs::ExecSkillResult result;
145 result.errmsg = errmsg;
150 fawkes_msgs::ExecSkillFeedback
151 RosSkillerThread::create_feedback()
153 return fawkes_msgs::ExecSkillFeedback();
162 if (!exec_running_ && !exec_request_
182 exec_request_ =
false;
192 exec_running_ =
true;
193 exec_msgid_ = msg->
id();
202 }
else if (exec_running_) {
203 if (exec_as_) as_goal_.publishFeedback(create_feedback());
205 if (skiller_if_->
status() == SkillerInterface::S_INACTIVE ||
206 skiller_if_->
msgid() != exec_msgid_)
211 if (loops_waited_ >= 3) {
214 std::string error_msg =
"Skiller doesn't start";
215 if (exec_as_) as_goal_.setAborted(create_result(error_msg), error_msg);
216 exec_running_ =
false;
219 else if (skiller_if_->
status() != SkillerInterface::S_RUNNING) {
220 exec_running_ =
false;
221 if (exec_as_ && exec_skill_string_ == skiller_if_->
skill_string()) {
222 if (skiller_if_->
status() == SkillerInterface::S_FINAL) {
223 std::string error_msg =
"Skill executed";
224 as_goal_.setSucceeded(create_result(error_msg), error_msg);
226 else if (skiller_if_->
status() == SkillerInterface::S_FAILED){
227 std::string error_msg =
"Failed to execute skill";
229 if (asprintf(&tmp,
"Failed to execute skill, error: %s", skiller_if_->
error()) != -1) {
233 as_goal_.setAborted(create_result(error_msg), error_msg);
240 fawkes_msgs::SkillStatus msg;
244 msg.error = skiller_if_->
error();
245 msg.status = skiller_if_->
status();
246 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.