action机制:SimpleActionClient

为方便同步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 GoalHandleT;

private:

typedef ActionClient ActionClientT;

// 一个GoalHandleT对应一个目标。一旦设置另一个目标,会先销毁原先目标。

GoalHandleT gh_;

boost::scoped_ptr ac_;

};1.1、构造ActionClient

typedef actionlib::SimpleActionClient MoveBaseClient;ActionClient是个模板类,构造时须要提供模板类型ActionSpec,像导航节点MoveBase中的move_base_msgs::MoveBaseAction。

// 个人建议简化为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::initSimpleClient(ros::NodeHandle & n, const std::string & name,

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::spinThread, this));

spin_thread_ = new net::tworker(boost::bind(&SimpleActionClient::spinThread, this), NULL, NULL, NULL, "ActionClientSpinThread");

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::sendGoal(const Goal & goal,

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::cancelGoal()

{

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::~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,client指actionlib::ActionClient

[server]初始化时,advertise“result”话题,归属对象是result_pub_。[client]初始化时,订阅“result”话题,收到该话题后,指定处理函数是ActionClientT::resultCb。[server]上层调用结束此次action的成员函数,包括setCanceled、setRejected、setAborted、setSucceeded,它们都会调用ActionServer::publishResult,后者执行的一个操作是result_pub_.publish(ar),即通过result_pub_发布一个“result”消息。[client]收到一个“result”消息,调用ActionClientT::resultCb进行处理。resultCb一个任务是要调用app在sendGoal时调置的done_cb,即图2中的done_cb_。图2 resultCb中的handleTransition图2显示了client收到result话题时,回调resultCb进行处理,后者会调用handleTransition。done_cb_是app在sendGoal时调置的done_cb。接下的进入SimpleGoalState::DONE,以及让done_condition_有信号,会让需要同步等待action完成的场景时,可以退出waitForResult,详细见“三、SimpleActionClient::waitForResult“。

handleTransition不仅处理收到result话题,也处理status。但feedback时较简单,不会改变client状态,不须要执行handleTransition。

三、SimpleActionClient::waitForResult

导航时,client向server发送一个目标点后,就不管server啥时处理完这个任务。但在一些场景,像moveit中的plan、execute。execute只能在plan结束后才能执行,而且在plan和execute中间,程序一般不允许做其它事,这时就要同步执行plan这个action。actionlib为方便处理同步场景,提供了waitForResult函数。

参数timeout指示等待溢出时间。值是0时,表示死等,直到cur_simple_state_进入SimpleGoalState::DONE状态。

template

bool SimpleActionClient::waitForResult(const ros::Duration & timeout)

{

...

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(time_left.toSec() * 1000.0f)));done_condition_有信号,或wait够了time_left秒,timed_wait就会结束。

}

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,是在两个线程。