move_base的服务端和客户端

move_base的服务端和客户端 | 沉默杀手

find_package中添加actionlib 和 actionlib_msgs

add_action_files(
   DIRECTORY action
   FILES
   DoDishes.action
 )

package.xml添加actionlib的编译和执行依赖，如下:

<build_depend>actionlib</build_depend>
<build_depend>actionlib_msgs</build_depend>
<run_depend>actionlib</run_depend>
<run_depend>actionlib_msgs</run_depend>

// action定义在 scheduler/action/Scheduler.action
typedef actionlib::SimpleActionServer<scheduler::SchedulerAction> Server;
actionlib::SimpleActionServer<scheduler::SchedulerAction>  as_;

// 注册抢占回调函数
as_.registerPreemptCallback(boost::bind(&Explore::preemptCB, this))
as_.registerGoalCallback(boost::bind(&Explore::goalCB, this));


void Explore::preemptCB()
{
  ROS_INFO("%s: Preempted", action_name_.c_str());
  explore_ = false;
  explore_state_ = STOP;
  ac_.cancelAllGoals();
  if(as_.isActive())
	as_.setPreempted();
}

void Explore::goalCB()
{
  explore_state_ = FULL_ROTATE;
  as_.acceptNewGoal();
}

SimpleActionServer有构造函数可以注册ExecuteCallback回调函数，但没有其他两个回调，原型：
SimpleActionServer (ros::NodeHandle n, std::string name, ExecuteCallback execute_callback, bool auto_start)

move_base的构造函数中是这样使用的：
as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);

服务端as_最后需要start()来启动，再加ros::spin()阻塞。 MoveBase构造函数的最后只有前者，ros::spin()其实是在move_base_node.cpp里

这里只看带回调函数的函数原型：

void actionlib::SimpleActionClient< ActionSpec >::sendGoal(const Goal& goal,
    SimpleDoneCallback       done_cb = SimpleDoneCallback(),
    SimpleActiveCallback     active_cb = SimpleActiveCallback(),
    SimpleFeedbackCallback   feedback_cb = SimpleFeedbackCallback() 
)

向ActionServer发送goal, 并注册回调函数。 如果调用本函数时已经有个goal处于active，状态机就会忽略goal并且开始向新的goal导航，不用发取消的请求。

• done_cb: Callback that gets called on transitions to Done，用于监听服务器任务执行完后的相应消息以及客户端的相应处理
• active_cb: Callback that gets called on transitions to Active，服务器任务被激活时的消息提示以及客户端的相应处理
• feedback_cb: Callback that gets called whenever feedback for this goal is received，接收服务器的反馈消息以及客户端的相应处理

  typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
  MoveBaseClient   ac_;
  
  // 示例
  ac_.sendGoal(goal,
      boost::bind(&DoDishesActionClient::DoneCb, this, _1, _2),
      boost::bind(&DoDishesActionClient::ActiveCb, this),
      boost::bind(&DoDishesActionClient::FeedbackCb, this, _1));
  
  
  void DoneCb(const actionlib::SimpleClientGoalState& state,
              const first_actionlib_sample::DoDishesResultConstPtr& result)
  {
          ROS_INFO("Finished in state [%s]", state.toString().c_str());
          ROS_INFO("Toal dish cleaned: %i", result->toal_dishes_cleaned);
          ros::shutdown();
  }
  
  // 当目标激活的时候，会调用一次
  void ActiveCb() 
  {
  	ROS_INFO("Goal just went active");
  }
  
  // 接收服务器的反馈信息
  void FeedbackCb(const first_actionlib_sample::DoDishesFeedbackConstPtr& feedback) {
      ROS_INFO("Got Feedback Complete Rate: %f", feedback->percent_complete);
  }
  

有用的函数：

cout<< ac_.getState().toString() <<endl;


// 获取goal的状态信息
// PENDING, ACTIVE, RECALLED, REJECTED, PREEMPTED, ABORTED, SUCCEEDED, LOST(isn't tracking a goal)
SimpleClientGoalState   getState () const
// 输出上面的几个状态，大写字母，对调试很有用
std::string toString() const

参考：ROS Action动作通讯编程