Ros

action机制：SimpleActionClient

ancientcc
  • 为方便同步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 ActionSpec>
class SimpleActionClient
{
  typedef ClientGoalHandle<ActionSpec> GoalHandleT;
private:
  typedef ActionClient<ActionSpec> ActionClientT;
  // 一个GoalHandleT对应一个目标。一旦设置另一个目标，会先销毁原先目标。
  GoalHandleT gh_;
  boost::scoped_ptr<ActionClientT> ac_;
};

1.1、构造ActionClient

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> 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<class ActionSpec>
void SimpleActionClient<ActionSpec>::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<ActionSpec>::spinThread, this));
    spin_thread_ = new net::tworker(boost::bind(&SimpleActionClient<ActionSpec>::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<class ActionSpec>
void SimpleActionClient<ActionSpec>::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<class ActionSpec>
void SimpleActionClient<ActionSpec>::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<ActionSpec>::~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<ActionSpec>，client指actionlib::ActionClient<ActionSpec>

  1. [server]初始化时，advertise“result”话题，归属对象是result_pub_。
  2. [client]初始化时，订阅“result”话题，收到该话题后，指定处理函数是ActionClientT::resultCb。
  3. [server]上层调用结束此次action的成员函数，包括setCanceled、setRejected、setAborted、setSucceeded，它们都会调用ActionServer<ActionSpec>::publishResult，后者执行的一个操作是result_pub_.publish(ar)，即通过result_pub_发布一个“result”消息。
  4. [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<ActionSpec>::waitForResult“。

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

 

三、SimpleActionClient<ActionSpec>::waitForResult

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

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

template<class ActionSpec>
bool SimpleActionClient<ActionSpec>::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<int64_t>(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，是在两个线程。

 

发布于 2022-03-13 16:12:30

全部评论: 0 条

    写评论: