move_base中的actionlib机制的源码探析

继续情怀系列。。。回忆做机器人的美好时光，那种无忧无虑的菜狗单纯的生活，同时也继续夯实软件开发的基础～

言归正传，相信看到这篇博文的同学们都不会对move_base陌生，它控制着全局规划和速度规划，它的具体流程在之前文章中我们已经仔细分析过，但还有一项仍然处于黑盒中，那就是和用户交互的actionlib。针对actionlib，有不少朋友写了关于它的分析，但大多都是从API角度以及它的状态机这些方向，几张流程图可能不能满足大家的胃口，今天我们从源码实现上仔细看看它的实现。

值得一提的是，actionlib并非只能用于运动控制，它适用于很多需要长时间监督并可以撤销的交互任务，只需提前设计好它的任务（即server中的响应函数）即可。

从使用上，actionlib封装地和ros service很类似，都是先定义一个client与server，并且它们的名称要对应上，同时，server带有一个回调函数以供执行。扩展性更强的是，它拥有一个状态机与对应的API，如源代码所示。本文的源代码位于ros源码包/src/actionlib下。

那么先从大家熟悉的move base开始，这里分别是在头文件与实现中的调用，move base拥有一个actionlib的server，并注册了一个execute callback函数。

typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;

as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

as_->start();

void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal)

一、服务器

template<class ActionSpec>
SimpleActionServer<ActionSpec>::SimpleActionServer(std::string name,
  ExecuteCallback execute_callback,
  bool auto_start)
: new_goal_(false), preempt_request_(false), new_goal_preempt_request_(false), execute_callback_(
    execute_callback), execute_thread_(NULL), need_to_terminate_(false)
{
  if (execute_callback_ != NULL) {
    execute_thread_ = new boost::thread(boost::bind(&SimpleActionServer::executeLoop, this));
  }

  // create the action server
  as_ = boost::shared_ptr<ActionServer<ActionSpec> >(new ActionServer<ActionSpec>(n_, name,
      boost::bind(&SimpleActionServer::goalCallback, this, _1),
      boost::bind(&SimpleActionServer::preemptCallback, this, _1),
      auto_start));
}

1.1 executeLoop函数在不断地轮转状态，以检测新的目标到来。

template<class ActionSpec>
void SimpleActionServer<ActionSpec>::executeLoop()
{
  ros::Duration loop_duration = ros::Duration().fromSec(.1);

  while (n_.ok()) {
    {
      boost::mutex::scoped_lock terminate_lock(terminate_mutex_);
      if (need_to_terminate_) {
        break;
      }
    }

    boost::recursive_mutex::scoped_lock lock(lock_);
    if (isActive()) {
      ROS_ERROR_NAMED("actionlib", "Should never reach this code with an active goal");
    } else if (isNewGoalAvailable()) {//new_goal_标志被置为true后，即goalcallback函数被执行完毕后
      GoalConstPtr goal = acceptNewGoal();

      ROS_FATAL_COND(!execute_callback_,
        "execute_callback_ must exist. This is a bug in SimpleActionServer");

      {
        // Make sure we're not locked when we call execute
        //重新以新的goal执行executecallback函数
        boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);
        execute_callback_(goal);
      }

      if (isActive()) {
        ROS_WARN_NAMED("actionlib", "Your executeCallback did not set the goal to a terminal status.\n"
          "This is a bug in your ActionServer implementation. Fix your code!\n"
          "For now, the ActionServer will set this goal to aborted");
        setAborted(
          Result(),
          "This goal was aborted by the simple action server. The user should have set a terminal status on this goal and did not");
      }
    } else {
      execute_condition_.timed_wait(lock,
        boost::posix_time::milliseconds(loop_duration.toSec() * 1000.0f));
    }
  }
}

1.2 ActionServer的构造在于initialize函数，它发布了三个话题，剧透地说，这正是客户端所接收的。因此，它们通过话题通信，利用若干个控制与反馈的话题起到松耦合控制的效果。

template<class ActionSpec>
void ActionServer<ActionSpec>::initialize()
{
  // read the queue size for each of the publish & subscribe components of the action
  // server
  int pub_queue_size;
  int sub_queue_size;
  node_.param("actionlib_server_pub_queue_size", pub_queue_size, 50);
  node_.param("actionlib_server_sub_queue_size", sub_queue_size, 50);
  if (pub_queue_size < 0) {pub_queue_size = 50;}
  if (sub_queue_size < 0) {sub_queue_size = 50;}

  result_pub_ = node_.advertise<ActionResult>("result", static_cast<uint32_t>(pub_queue_size));
  feedback_pub_ =
    node_.advertise<ActionFeedback>("feedback", static_cast<uint32_t>(pub_queue_size));
  status_pub_ =
    node_.advertise<actionlib_msgs::GoalStatusArray>("status",
      static_cast<uint32_t>(pub_queue_size), true);

  // read the frequency with which to publish status from the parameter server
  // if not specified locally explicitly, use search param to find actionlib_status_frequency
  double status_frequency, status_list_timeout;
  if (!node_.getParam("status_frequency", status_frequency)) {
    std::string status_frequency_param_name;
    if (!node_.searchParam("actionlib_status_frequency", status_frequency_param_name)) {
      status_frequency = 5.0;
    } else {
      node_.param(status_frequency_param_name, status_frequency, 5.0);
    }
  } else {
    ROS_WARN_NAMED("actionlib",
      "You're using the deprecated status_frequency parameter, please switch to actionlib_status_frequency.");
  }
  node_.param("status_list_timeout", status_list_timeout, 5.0);

  this->status_list_timeout_ = ros::Duration(status_list_timeout);
  //创造一个Timer轮循地发布服务器状态
  if (status_frequency > 0) {
    status_timer_ = node_.createTimer(ros::Duration(1.0 / status_frequency),
        boost::bind(&ActionServer::publishStatus, this, _1));
  }
   //接收来自客户端的话题，关于新旧目标点的控制
  goal_sub_ = node_.subscribe<ActionGoal>("goal", static_cast<uint32_t>(sub_queue_size),
      boost::bind(&ActionServerBase<ActionSpec>::goalCallback, this, _1));

  cancel_sub_ =
    node_.subscribe<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(sub_queue_size),
      boost::bind(&ActionServerBase<ActionSpec>::cancelCallback, this, _1));
}

actionlib为每个 goal设置了一个状态跟踪器，每个目标有一个独有的id，这是状态发布与取消goal的基础。 

std::list<StatusTracker<ActionSpec> > status_list_;

1.3 goalcallback函数

template<class ActionSpec>
void SimpleActionServer<ActionSpec>::goalCallback(GoalHandle goal)
{
  boost::recursive_mutex::scoped_lock lock(lock_);
  ROS_DEBUG_NAMED("actionlib", "A new goal has been recieved by the single goal action server");

  // check that the timestamp is past or equal to that of the current goal and the next goal
  if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp) &&
    (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp))
  {
    // if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
    if (next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)) {
      next_goal_.setCanceled(
        Result(),
        "This goal was canceled because another goal was recieved by the simple action server");
    }

    next_goal_ = goal;
    new_goal_ = true;
    new_goal_preempt_request_ = false;

    // if the server is active, we'll want to call the preempt callback for the current goal
    if (isActive()) {
      preempt_request_ = true;
      // if the user has registered a preempt callback, we'll call it now
      if (preempt_callback_) {
        preempt_callback_();
      }
    }

    // if the user has defined a goal callback, we'll call it now
    //需要显式地设计该函数
    if (goal_callback_) {
      goal_callback_();
    }

    // Trigger runLoop to call execute()
    //唤醒executeLoop函数中等待的条件变量
    execute_condition_.notify_all();
  } else {
    // the goal requested has already been preempted by a different goal, so we're not going to execute it
    goal.setCanceled(
      Result(),
      "This goal was canceled because another goal was recieved by the simple action server");
  }
}

而当需要进行抢占行为的时候，调用preemptcallback函数。当然，preempt_callback_也需要显式地制定，如果没有设计这个函数则乖乖被强占。

template<class ActionSpec>
void SimpleActionServer<ActionSpec>::preemptCallback(GoalHandle preempt)
{
  boost::recursive_mutex::scoped_lock lock(lock_);
  ROS_DEBUG_NAMED("actionlib", "A preempt has been received by the SimpleActionServer");

  // if the preempt is for the current goal, then we'll set the preemptRequest flag and call the user's preempt callback
  if (preempt == current_goal_) {
    ROS_DEBUG_NAMED("actionlib",
      "Setting preempt_request bit for the current goal to TRUE and invoking callback");
    preempt_request_ = true;

    // if the user has registered a preempt callback, we'll call it now
    if (preempt_callback_) {
      preempt_callback_();
    }
  } else if (preempt == next_goal_) {
    // if the preempt applies to the next goal, we'll set the preempt bit for that
    ROS_DEBUG_NAMED("actionlib", "Setting preempt request bit for the next goal to TRUE");
    new_goal_preempt_request_ = true;
  }
}

1.4 cancelcallback 函数

template<class ActionSpec>
void ActionServerBase<ActionSpec>::cancelCallback(
  const boost::shared_ptr<const actionlib_msgs::GoalID> & goal_id)
{
  boost::recursive_mutex::scoped_lock lock(lock_);

  // if we're not started... then we're not actually going to do anything
  if (!started_) {
    return;
  }

  // we need to handle a cancel for the user
  ROS_DEBUG_NAMED("actionlib", "The action server has received a new cancel request");
  bool goal_id_found = false;
  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.begin();
    it != status_list_.end(); ++it)
  {
    // check if the goal id is zero or if it is equal to the goal id of
    // the iterator or if the time of the iterator warrants a cancel
    if (
      (goal_id->id == "" && goal_id->stamp == ros::Time()) ||  // id and stamp 0 --> cancel everything
      goal_id->id == (*it).status_.goal_id.id ||    // ids match... cancel that goal
      (goal_id->stamp != ros::Time() && (*it).status_.goal_id.stamp <= goal_id->stamp)       // stamp != 0 --> cancel everything before stamp
    )
    {
      // we need to check if we need to store this cancel request for later
      if (goal_id->id == (*it).status_.goal_id.id) {
        goal_id_found = true;
      }

      // attempt to get the handle_tracker for the list item if it exists
      boost::shared_ptr<void> handle_tracker = (*it).handle_tracker_.lock();

      if ((*it).handle_tracker_.expired()) {
        // if the handle tracker is expired, then we need to create a new one
        HandleTrackerDeleter<ActionSpec> d(this, it, guard_);
        handle_tracker = boost::shared_ptr<void>((void *)NULL, d);
        (*it).handle_tracker_ = handle_tracker;

        // we also need to reset the time that the status is supposed to be removed from the list
        (*it).handle_destruction_time_ = ros::Time();
      }

      // set the status of the goal to PREEMPTING or RECALLING as approriate
      // and check if the request should be passed on to the user
      GoalHandle gh(it, this, handle_tracker, guard_);
      if (gh.setCancelRequested()) {
        // make sure that we're unlocked before we call the users callback
        boost::reverse_lock<boost::recursive_mutex::scoped_lock> unlocker(lock);

        // call the user's cancel callback on the relevant goal
        cancel_callback_(gh);
      }
    }
  }

  // if the requested goal_id was not found, and it is non-zero, then we need to store the cancel request
  if (goal_id->id != "" && !goal_id_found) {
    typename std::list<StatusTracker<ActionSpec> >::iterator it = status_list_.insert(
      status_list_.end(),
      StatusTracker<ActionSpec>(*goal_id, actionlib_msgs::GoalStatus::RECALLING));
    // start the timer for how long the status will live in the list without a goal handle to it
    (*it).handle_destruction_time_ = goal_id->stamp;
  }

  // make sure to set last_cancel_ based on the stamp associated with this cancel request
  if (goal_id->stamp > last_cancel_) {
    last_cancel_ = goal_id->stamp;
  }
}

template<class ActionSpec>
bool ServerGoalHandle<ActionSpec>::setCancelRequested()
{
  if (as_ == NULL) {
    ROS_ERROR_NAMED("actionlib",
      "You are attempting to call methods on an uninitialized goal handle");
    return false;
  }

  // check to see if we can use the action server
  DestructionGuard::ScopedProtector protector(*guard_);
  if (!protector.isProtected()) {
    ROS_ERROR_NAMED("actionlib",
      "The ActionServer associated with this GoalHandle is invalid. Did you delete the ActionServer before the GoalHandle?");
    return false;
  }

  ROS_DEBUG_NAMED("actionlib",
    "Transitioning to a cancel requested state on goal id: %s, stamp: %.2f",
    getGoalID().id.c_str(), getGoalID().stamp.toSec());
  if (goal_) {
    boost::recursive_mutex::scoped_lock lock(as_->lock_);
    unsigned int status = (*status_it_).status_.status;
    if (status == actionlib_msgs::GoalStatus::PENDING) {
      (*status_it_).status_.status = actionlib_msgs::GoalStatus::RECALLING;
      as_->publishStatus();
      return true;
    }

    if (status == actionlib_msgs::GoalStatus::ACTIVE) {
      (*status_it_).status_.status = actionlib_msgs::GoalStatus::PREEMPTING;
      as_->publishStatus();
      return true;
    }
  }
  return false;
}

 

1.5 最后就是发布状态消息的函数，它是通过ros的Timer轮转地发布信息。

template<class ActionSpec>
void ActionServer<ActionSpec>::publishStatus()
{
  boost::recursive_mutex::scoped_lock lock(this->lock_);
  // build a status array
  actionlib_msgs::GoalStatusArray status_array;

  status_array.header.stamp = ros::Time::now();

  status_array.status_list.resize(this->status_list_.size());

  unsigned int i = 0;
  for (typename std::list<StatusTracker<ActionSpec> >::iterator it = this->status_list_.begin();
    it != this->status_list_.end(); )
  {
    status_array.status_list[i] = (*it).status_;

    // check if the item is due for deletion from the status list
    if ((*it).handle_destruction_time_ != ros::Time() &&
      (*it).handle_destruction_time_ + this->status_list_timeout_ < ros::Time::now())
    {
      it = this->status_list_.erase(it);
    } else {
      ++it;
    }
    ++i;
  }

  status_pub_.publish(status_array);
}

而movebase中的start函数，代表着初始化以及开始发布状态消息，实现上述过程描述的服务器启动过程。

template<class ActionSpec>
void ActionServerBase<ActionSpec>::start()
{
  initialize();
  started_ = true;
  publishStatus();
}

二、客户端

相比于服务器，客户端的实现要略微复杂一些。actionlib为客户端提供了若干API，使用户可以适时开启、结束任务，也可以在执行过程中查看它反馈的执行状态。我们先看客户端的实现：

  ActionClient(const ros::NodeHandle & n, const std::string & name,
    ros::CallbackQueueInterface * queue = NULL)
  : n_(n, name), guard_(new DestructionGuard()),
    manager_(guard_)
  {
    initClient(queue);
  }

  void initClient(ros::CallbackQueueInterface * queue)
  {
    ros::Time::waitForValid();
    // read parameters indicating publish/subscribe queue sizes
    int pub_queue_size;
    int sub_queue_size;
    n_.param("actionlib_client_pub_queue_size", pub_queue_size, 10);
    n_.param("actionlib_client_sub_queue_size", sub_queue_size, 1);
    if (pub_queue_size < 0) {pub_queue_size = 10;}
    if (sub_queue_size < 0) {sub_queue_size = 1;}
    
    //在这里queue_subscribe和queue_advertise都是模仿发布器和接收器设置的，可以把它们当做一般的ros话题理解
    //接收反馈的状态
    status_sub_ = queue_subscribe("status", static_cast<uint32_t>(sub_queue_size),
        &ActionClientT::statusCb, this, queue);
    feedback_sub_ = queue_subscribe("feedback", static_cast<uint32_t>(sub_queue_size),
        &ActionClientT::feedbackCb, this, queue);
    result_sub_ = queue_subscribe("result", static_cast<uint32_t>(sub_queue_size),
        &ActionClientT::resultCb, this, queue);

    connection_monitor_.reset(new ConnectionMonitor(feedback_sub_, result_sub_));

    // Start publishers and subscribers
    //发送控制指令话题
    goal_pub_ = queue_advertise<ActionGoal>("goal", static_cast<uint32_t>(pub_queue_size),
        boost::bind(&ConnectionMonitor::goalConnectCallback, connection_monitor_, _1),
        boost::bind(&ConnectionMonitor::goalDisconnectCallback, connection_monitor_, _1),
        queue);
    cancel_pub_ =
      queue_advertise<actionlib_msgs::GoalID>("cancel", static_cast<uint32_t>(pub_queue_size),
        boost::bind(&ConnectionMonitor::cancelConnectCallback, connection_monitor_, _1),
        boost::bind(&ConnectionMonitor::cancelDisconnectCallback, connection_monitor_, _1),
        queue);
    //注册控制goal的函数
    manager_.registerSendGoalFunc(boost::bind(&ActionClientT::sendGoalFunc, this, _1));
    manager_.registerCancelFunc(boost::bind(&ActionClientT::sendCancelFunc, this, _1));
  }

看到这里我们可能也猜到了，它在用boost::function注册两个控制函数，而这两个函数正是调用publish的函数，它们在触发回调函数时发送goal或取消goal的话题给服务器。

void GoalManager<ActionSpec>::registerSendGoalFunc(SendGoalFunc send_goal_func)
{
  send_goal_func_ = send_goal_func;
}

  void sendGoalFunc(const ActionGoalConstPtr & action_goal)
  {
    goal_pub_.publish(action_goal);
  }
​

template<class ActionSpec>
void GoalManager<ActionSpec>::registerCancelFunc(CancelFunc cancel_func)
{
  cancel_func_ = cancel_func;
}

 接下来就是客户端的各种api了，在控制actionlib的时候一般会用到发送、取消、监视状态等功能。

2.1 发送任务通过sendGoal函数控制，定位到simple_action_client.h，这个sendGoal的三个形参我们一般无法显式地制定。

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));
}

  GoalHandle sendGoal(const Goal & goal,
    TransitionCallback transition_cb = TransitionCallback(),
    FeedbackCallback feedback_cb = FeedbackCallback())
  {
    ROS_DEBUG_NAMED("actionlib", "about to start initGoal()");
    GoalHandle gh = manager_.initGoal(goal, transition_cb, feedback_cb);
    ROS_DEBUG_NAMED("actionlib", "Done with initGoal()");

    return gh;
  }

 转到goal_manager_iml.h，可以看到initGoal的实现：

template<class ActionSpec>
ClientGoalHandle<ActionSpec> GoalManager<ActionSpec>::initGoal(const Goal & goal,
  TransitionCallback transition_cb,
  FeedbackCallback feedback_cb)
{
  ActionGoalPtr action_goal(new ActionGoal);
  action_goal->header.stamp = ros::Time::now();
  action_goal->goal_id = id_generator_.generateID();
  action_goal->goal = goal;

  typedef CommStateMachine<ActionSpec> CommStateMachineT;
  boost::shared_ptr<CommStateMachineT> comm_state_machine(new CommStateMachineT(action_goal,
    transition_cb,
    feedback_cb));

  boost::recursive_mutex::scoped_lock lock(list_mutex_);
  typename ManagedListT::Handle list_handle =
    list_.add(comm_state_machine, boost::bind(&GoalManagerT::listElemDeleter, this, _1), guard_);

  if (send_goal_func_) {
    send_goal_func_(action_goal);
  } else {
    ROS_WARN_NAMED("actionlib",
      "Possible coding error: send_goal_func_ set to NULL. Not going to send goal");
  }

  return GoalHandleT(this, list_handle, guard_);
}

2.2 结束任务可以调用cancelGoal函数，也是通过发布取消的话题实现的。

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();
}

template<class ActionSpec>
void ClientGoalHandle<ActionSpec>::cancel()
{
  if (!active_) {
    ROS_ERROR_NAMED("actionlib",
      "Trying to cancel() on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
    return;
  }

  assert(gm_);
  if (!gm_)
  {
    ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
    return;
  }

  DestructionGuard::ScopedProtector protector(*guard_);
  if (!protector.isProtected()) {
    ROS_ERROR_NAMED("actionlib",
      "This action client associated with the goal handle has already been destructed. Ignoring this call");
    return;
  }

  boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);

  switch (list_handle_.getElem()->getCommState().state_) {
    case CommState::WAITING_FOR_GOAL_ACK:
    case CommState::PENDING:
    case CommState::ACTIVE:
    case CommState::WAITING_FOR_CANCEL_ACK:
      break;  // Continue standard processing
    case CommState::WAITING_FOR_RESULT:
    case CommState::RECALLING:
    case CommState::PREEMPTING:
    case CommState::DONE:
      ROS_DEBUG_NAMED("actionlib", "Got a cancel() request while in state [%s], so ignoring it",
        list_handle_.getElem()->getCommState().toString().c_str());
      return;
    default:
      ROS_ERROR_NAMED("actionlib", "BUG: Unhandled CommState: %u",
        list_handle_.getElem()->getCommState().state_);
      return;
  }

  ActionGoalConstPtr action_goal = list_handle_.getElem()->getActionGoal();

  actionlib_msgs::GoalID cancel_msg;
  cancel_msg.stamp = ros::Time(0, 0);
  cancel_msg.id = list_handle_.getElem()->getActionGoal()->goal_id.id;
  //这里就是注册的取消goal的回调函数，直接发送取消的话题给服务器
  if (gm_->cancel_func_) {
    gm_->cancel_func_(cancel_msg);
  }

  list_handle_.getElem()->transitionToState(*this, CommState::WAITING_FOR_CANCEL_ACK);
}

2.3 监听服务器状态

在刚才的分析中我们也发现了状态机的存在，而actionlib定义了三种状态枚举，分别是暂停、运行中、完成状态，我们可以时刻监听这些状态。另外，actionlib_msgs::GoalStatus也定义了若干状态，包括recalling，preepting等。

  enum StateEnum
  {
    PENDING,
    ACTIVE,
    DONE
  };

template<class ActionSpec>
CommState ClientGoalHandle<ActionSpec>::getCommState() const
{
  assert(gm_);
  if (!gm_) {
    ROS_ERROR_NAMED("actionlib", "Client should have valid GoalManager");
    return CommState(CommState::DONE);
  }

  boost::recursive_mutex::scoped_lock lock(gm_->list_mutex_);
  if (!active_) {
    ROS_ERROR_NAMED("actionlib",
      "Trying to getCommState on an inactive ClientGoalHandle. You are incorrectly using a ClientGoalHandle");
    return CommState(CommState::DONE);
  }

  DestructionGuard::ScopedProtector protector(*guard_);
  if (!protector.isProtected()) {
    ROS_ERROR_NAMED("actionlib",
      "This action client associated with the goal handle has already been destructed. Ignoring this getCommState() call");
    return CommState(CommState::DONE);
  }

  return list_handle_.getElem()->getCommState();
}

我们是通过内部传递的话题status获得服务器的状态，但它提供了一个获得状态的API，我们直接调用得到当下的状态。

template<class ActionSpec>
CommState CommStateMachine<ActionSpec>::getCommState() const
{
  return state_;
}

 接下来是对状态的处理，actionlib利用回调函数获得服务器传递的状态，并对一些状态进行合并与转换，提供给客户使用。

  void statusCb(const ros::MessageEvent<actionlib_msgs::GoalStatusArray const> & status_array_event)
  {
    ROS_DEBUG_NAMED("actionlib", "Getting status over the wire.");
    if (connection_monitor_) {
      connection_monitor_->processStatus(
        status_array_event.getConstMessage(), status_array_event.getPublisherName());
    }
    manager_.updateStatuses(status_array_event.getConstMessage());
  }

template<class ActionSpec>
void CommStateMachine<ActionSpec>::updateStatus(GoalHandleT & gh,
  const actionlib_msgs::GoalStatusArrayConstPtr & status_array)
{
  const actionlib_msgs::GoalStatus * goal_status = findGoalStatus(status_array->status_list);

  // It's possible to receive old GoalStatus messages over the wire, even after receiving Result with a terminal state.
  //   Thus, we want to ignore all status that we get after we're done, because it is irrelevant. (See trac #2721)
  if (state_ == CommState::DONE) {
    return;
  }

  if (goal_status) {
    latest_goal_status_ = *goal_status;
  } else {
    if (state_ != CommState::WAITING_FOR_GOAL_ACK &&
      state_ != CommState::WAITING_FOR_RESULT &&
      state_ != CommState::DONE)
    {
      processLost(gh);
    }
    return;
  }

  switch (state_.state_) {
    case CommState::WAITING_FOR_GOAL_ACK:
      {
        if (goal_status) {
          switch (goal_status->status) {
            case actionlib_msgs::GoalStatus::PENDING:
              transitionToState(gh, CommState::PENDING);
              break;
            case actionlib_msgs::GoalStatus::ACTIVE:
              transitionToState(gh, CommState::ACTIVE);
              break;
            case actionlib_msgs::GoalStatus::PREEMPTED:
              transitionToState(gh, CommState::ACTIVE);
              transitionToState(gh, CommState::PREEMPTING);
              transitionToState(gh, CommState::WAITING_FOR_RESULT);
              break;
            case actionlib_msgs::GoalStatus::SUCCEEDED:
              transitionToState(gh, CommState::ACTIVE);
              transitionToState(gh, CommState::WAITING_FOR_RESULT);
              break;
            case actionlib_msgs::GoalStatus::ABORTED:
              transitionToState(gh, CommState::ACTIVE);
              transitionToState(gh, CommState::WAITING_FOR_RESULT);
              break;
            case actionlib_msgs::GoalStatus::REJECTED:
              transitionToState(gh, CommState::PENDING);
              transitionToState(gh, CommState::WAITING_FOR_RESULT);
              break;
            case actionlib_msgs::GoalStatus::RECALLED:
              transitionToState(gh, CommState::PENDING);
              transitionToState(gh, CommState::WAITING_FOR_RESULT);
              break;
            case actionlib_msgs::GoalStatus::PREEMPTING:
              transitionToState(gh, CommState::ACTIVE);
              transitionToState(gh, CommState::PREEMPTING);
              break;
            case actionlib_msgs::GoalStatus::RECALLING:
              transitionToState(gh, CommState::PENDING);
              transitionToState(gh, CommState::RECALLING);
              break;
            default:
              ROS_ERROR_NAMED("actionlib",
                "BUG: Got an unknown status from the ActionServer. status = %u",
                goal_status->status);
              break;
          }
        }
        break;
      }
。。。。。

通过状态的转换获得最终的状态：

template<class ActionSpec>
void CommStateMachine<ActionSpec>::transitionToState(GoalHandleT & gh, const CommState & next_state)
{
  ROS_DEBUG_NAMED("actionlib", "Trying to transition to %s", next_state.toString().c_str());
  setCommState(next_state);
  if (transition_cb_) {
    transition_cb_(gh);
  }
}

 最后做一个总结就是，不出意料地actionlib是通过多个话题的形式实现控制，但是内部实现的复杂度略微超出了想象，由于它支持多个客户端同时对服务器发指令，内部必须维护一个list来控制多个目标点的实施。里面还有若干部分做了安全方面的冗余，对同学们来说，可以称得上是软件开发中一个模块的典范～