为方便同步action机制的三个cb回调,建议使用三个参数的SimpleActionClient构造版本,并且spin_thread = false。这样还可以少创建一个线程。SimpleActionClient任一时刻最多设置一个目标。gh_.reset()、gh_.cancel()主要区别:cancel()会触发action相关逻辑,使得caller收到逻辑中的回调,像done_cb。一次action涉及到5个话题。话题名称由两部分组成,第一部分node名称,第二部分那5个短名称,像/launcher/result。对于发生在每个node内的action实例,它们共用这5个名称。用waitForResult进行同步时,不要让调用waitForResult的线程和处理SimpleActionClient中CallbackQueue的是同一个线程。一、actionlib::SimpleActionClient
为简化action机制client端编程,ros提供了actionlib::ActionClient、actionlib::ClientGoalHandle,但开发者直接使用它们还是烦锁,于是在这之上再提供SimpleActionClient,ActionClient、ClientGoalHandle是SimpleActionClient的两个成员变量。
template
class SimpleActionClient
{
typedef ClientGoalHandle
private:
typedef ActionClient
// 一个GoalHandleT对应一个目标。一旦设置另一个目标,会先销毁原先目标。
GoalHandleT gh_;
boost::scoped_ptr
};1.1、构造ActionClient
typedef actionlib::SimpleActionClient
// 个人建议简化为SimpleActionClient(const std::string & name), spin_thread固定是true。
// 使用场景:caller希望由SimpleActionClient提供CallbackQueue,三个cb在spin_thread的上下文执行。
SimpleActionClient(const std::string & name, bool spin_thread = true)
: cur_simple_state_(SimpleGoalState::PENDING)
{
initSimpleClient(nh_, name, spin_thread);
}
// 个人建议简化为SimpleActionClient(ros::NodeHandle& n, const std::string& name), spin_thread固定是false。
// 使用场景:caller自已提供CallbackQueue,三个cb在自已能方便同步的线程上下文执行,像MainThread。
SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)
: cur_simple_state_(SimpleGoalState::PENDING)
{
initSimpleClient(n, name, spin_thread);
}SimpleActionClient提供了两种构造函数,个人认为有点搞复杂了。可以把caller使用SimpleActionClient分为两种场景。
第一个构造函数,spin_thread = true。使用场景:caller希望由SimpleActionClient提供CallbackQueue,三个cb在spin_thread的上下文执行。第二个构造函数,spin_thread = false。使用场景:caller自已提供CallbackQueue,三个cb在自已能方便同步的线程上下文执行,像MainThread。不建议其它的构造函数、参数组合,像使用第一个构造函数,spin_thread却置了false。不论哪种构造函数,都调用initSimpleClient。
template
void SimpleActionClient
bool spin_thread)
{
if (spin_thread) {
ROS_DEBUG_NAMED("actionlib", "Spinning up a thread for the SimpleActionClient");
need_to_terminate_ = false;
// spin_thread_ = new boost::thread(boost::bind(&SimpleActionClient
spin_thread_ = new net::tworker(boost::bind(&SimpleActionClient
ac_.reset(new ActionClientT(n, name, &callback_queue));
} else {
spin_thread_ = NULL;
ac_.reset(new ActionClientT(n, name, n.getCallbackQueue()));
}
}ac_类型是ActionClient,在概造ActionClient时,会订阅action机制的三种话题:status、feedback和result,同时发布两种话题:goal、cancel。它们的回调便是三种订阅话题对应的三个cb,它们在ActionClient第三个参数CallbackQueue的上下文执行。
1.2、设置目标
template
void SimpleActionClient
SimpleDoneCallback done_cb,
SimpleActiveCallback active_cb,
SimpleFeedbackCallback feedback_cb)
{
// Reset the old GoalHandle, so that our callbacks won't get called anymore
gh_.reset();
// Store all the callbacks
done_cb_ = done_cb;
active_cb_ = active_cb;
feedback_cb_ = feedback_cb;
cur_simple_state_ = SimpleGoalState::PENDING;
// Send the goal to the ActionServer
gh_ = ac_->sendGoal(goal, boost::bind(&SimpleActionClientT::handleTransition, this, _1),
boost::bind(&SimpleActionClientT::handleFeedback, this, _1, _2));
}可认为gh_对应一个目标,sendGoal首先复位它,意味着SimpleActionClient任一时刻最多设置一个目标。注意gb_.reset()不会触发done_cb。
1.3、取消目标
template
void SimpleActionClient
{
if (gh_.isExpired()) {
ROS_ERROR_NAMED("actionlib",
"Trying to cancelGoal() when no goal is running. You are incorrectly using SimpleActionClient");
}
gh_.cancel();
}和gh_.reset()一样,gh_.cancel()也会de-active掉gh_,但它会触发action相关逻辑,便得caller收到逻辑中的回调,像done_cb。
1.4、销毁SimpleActionClient
SimpleActionClient
{
if (spin_thread_) {
{
boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
need_to_terminate_ = true;
}
// spin_thread_->join();
delete spin_thread_;
}
gh_.reset();
ac_.reset();
}1)销毁spin_thread_。2)复位目标。3)销毁ac_。
二、话题
图1 action涉及到5话题图1显示了一次action会涉及到的5个话题。action话题名称由两部分组成,第一部分node名称,第二部分那5个短名称。对于发生在每个node内的action实例,它们共用这个5个名称。
goal:发布任务目标。cancel:请求取消任务。status:通知client当前的状态。feedback:周期反馈任务运行的监控数据。result:此次任务处理结束,向Client发送任务的执行结果,只发布一次。以result为例,看大概流程。server指actionlib::ActionServer
[server]初始化时,advertise“result”话题,归属对象是result_pub_。[client]初始化时,订阅“result”话题,收到该话题后,指定处理函数是ActionClientT::resultCb。[server]上层调用结束此次action的成员函数,包括setCanceled、setRejected、setAborted、setSucceeded,它们都会调用ActionServer
handleTransition不仅处理收到result话题,也处理status。但feedback时较简单,不会改变client状态,不须要执行handleTransition。
三、SimpleActionClient
导航时,client向server发送一个目标点后,就不管server啥时处理完这个任务。但在一些场景,像moveit中的plan、execute。execute只能在plan结束后才能执行,而且在plan和execute中间,程序一般不允许做其它事,这时就要同步执行plan这个action。actionlib为方便处理同步场景,提供了waitForResult函数。
参数timeout指示等待溢出时间。值是0时,表示死等,直到cur_simple_state_进入SimpleGoalState::DONE状态。
template
bool SimpleActionClient
{
...
while (nh_.ok()) {
...
if (cur_simple_state_ == SimpleGoalState::DONE) {
break;若是死等,是在这个入口退出while循环,见图2,它在把done_condition_置为有信号前,会先把cur_simple_state_置为SimpleGoalState::DONE。
}
...
done_condition_.timed_wait(lock,
boost::posix_time::milliseconds(static_cast
}
return cur_simple_state_ == SimpleGoalState::DONE;
}调用这函数时要注意:不要让调用waitForResult的线程和处理SimpleActionClient中CallbackQueue的是同一个线程。假设它们都在main thread线程,让看下面这个流程。
[client的main thread]向server发送完goal后,调用waitForResult等待任务结束。[server]任务执行结束,发出result话题。[client的main thread]阻塞在waitForResult,不可能去查询SimpleActionClient的CallbackQueue,导致无法回调resultCb,也就无法使得cur_simple_state_进入SimpleGoalState::DONE,程序死锁。正是这原因,编写moveit::planning_interface::MoveGroupInterface时,往往用AsyncSpinner,把它的CallbackQueue传给SimpleActionClient,从而让waitForResult和SimpleActionClient的CallbackQueue,是在两个线程。