ROS之actionlib

1，什么是action

   用过toplic的同学们都知道，两个节点之间可以通过发布或者订阅来单向通信。但是如果我们在发布信息到指定的节点的同时，我们需要知道订阅的节点是否接收到消息，执行状态如何，那么问题来了，我是否可以通过节点中同时发布和订阅主题的执行状态呢？答案当然是可以的，action就解决了这一痛点；

  action机制从原理上看的话，其实是基于topic实现的，相当于制定了一套由目标话题，反馈话题，结果话题组成的高级协议。action实现了一种类似于service的请求/响应通讯机制，区别在于action带有反馈机制，用来不断向客户端反馈任务的进度，并且还支持在任务中途中止运行。
操作起来就像这样子，客户端给服务端抛出一个目标，然后客户端就可以去干别的事情了，在任务执行期间，客户端会以消息的形式，周期性地接收到来自服务端的进度反馈，如果没有终止任务的话这个过程会一直延续到收到最终的结果。当然也可以随时终止当前的操作，开始一个全新的操作；

1，创建功能包

catkin_create_pkg action_demo roscpp actionlib actionlib_msgs  //创建功能包
cd action_demo/
mkdir action  //新建action文件夹来存放.action文件

2，action文件

action文件格式

#part1: the goal, to be sent by the client 
--- 
#part2: the result, to be sent by the server upon completion 
--- 
#part3: the feedback, to be sent by server during execution
---

cd action/&&gedit action_demo.action   //新建.action文件

加入如下内容

# Goal
int32 goal
---
# Result
bool result
---
# Feedback message
int32 feedback_msg

打开CMakeList.txt，加入下面语句

## Generate actions in the 'action' folder
add_action_files(
  FILES
  action_demo.action
)

## Generate added messages and services with any dependencies listed here
generate_messages(
  DEPENDENCIES
  actionlib_msgs
)
catkin_package(
  CATKIN_DEPENDS actionlib_msgs
)

接下来，编译！！

编译成功后，会生产.msg文件，如下

banker@banker:~$ ls catkin_ws/devel/share/action_demo/msg/
action_demoActionFeedback.msg  action_demoFeedback.msg
action_demoActionGoal.msg      action_demoGoal.msg
action_demoAction.msg          action_demoResult.msg
action_demoActionResult.msg
banker@banker:~$ ls catkin_ws/devel/include/action_demo/
action_demoActionFeedback.h  action_demoActionResult.h  action_demoResult.h
action_demoActionGoal.h      action_demoFeedback.h
action_demoAction.h          action_demoGoal.h

2，开始编写代码测试

server代码

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <action_demo/action_demoAction.h>

class action_demoAction
{
protected:

  ros::NodeHandle nh_;
  actionlib::SimpleActionServer<action_demo::action_demoAction> as_; // NodeHandle instance must be created before this line. Otherwise strange error occurs.
  std::string action_name_;
  // create messages that are used to published feedback/result
  action_demo::action_demoFeedback feedback_;
  action_demo::action_demoResult result_;

public:

  action_demoAction(std::string name) :
    as_(nh_, name, boost::bind(&action_demoAction::executeCB, this, _1), false),
    action_name_(name)
  {
    as_.start();
  }

  ~action_demoAction(void)
  {
  }

  void executeCB(const action_demo::action_demoGoalConstPtr &goal)
  {
    ros::Rate r(10);
    bool success = true;

    // publish info to the console for the user
    ROS_INFO("%s: Executing, creating actiondemo  order %i with feedback_msg %i", action_name_.c_str(), goal->goal, feedback_.feedback_msg);
    feedback_.feedback_msg = 0;
    result_.result = 0;
    // start executing the action
    for(int i=1; i<=goal->goal; i++)
    {

      feedback_.feedback_msg = i;
      result_.result += i;
      ROS_INFO("%i: feedback_.feedback_msg", feedback_.feedback_msg);
      // publish the feedback
      as_.publishFeedback(feedback_);

      r.sleep();
    }

    if(success)
    {
      //result_.result = feedback_.feedback_msg;
      ROS_INFO("%s: Succeeded", action_name_.c_str());
      // set the action state to succeeded
      as_.setSucceeded(result_);
    }
  }


};


int main(int argc, char** argv)
{
  ros::init(argc, argv, "action_demo");
  ROS_INFO(" action server start.");
  action_demoAction action_demo("action_demo");
  ros::spin();

  return 0;
}

client代码

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <action_demo/action_demoAction.h>

// 当action完成后会调用次回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,const action_demo::action_demoResultConstPtr& result)
{
    ROS_INFO("doneCb ,action finsh , result = %i",result->result);
    ros::shutdown();
}

 

// 当action激活后会调用次回调函数一次
void activeCb()
{

    ROS_INFO("Goal just went active");

}
// 收到feedback后调用的回调函数
void feedbackCb(const action_demo::action_demoFeedbackConstPtr& feedback)
{

    ROS_INFO(" feedback_msg : %i ", feedback->feedback_msg);

}

int main (int argc, char **argv)
{
  ros::init(argc, argv, "action_demo_client");

  // create the action client
  // true causes the client to spin its own thread
  actionlib::SimpleActionClient<action_demo::action_demoAction> ac("action_demo", true);

  ROS_INFO("Waiting for action server to start.");
  // wait for the action server to start
  ac.waitForServer(); //will wait for infinite time

  ROS_INFO("Action server started, sending goal.");
  // send a goal to the action
  action_demo::action_demoGoal goal;
  goal.goal = 5;
  //ac.sendGoal(goal);
  ac.sendGoal(goal,  &doneCb, &activeCb, &feedbackCb);
  //wait for the action to return
  bool finished_before_timeout = ac.waitForResult(ros::Duration(10.0));

  if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
  }
  else
    ROS_INFO("Action did not finish before the time out.");

  //exit
  return 0;
}

修改CMakeLists，加入如下代码：

add_executable(action_demo_server src/action_demo_server.cpp)

add_executable(action_demo_client src/action_demo_client.cpp)

target_link_libraries(
  action_demo_client
  ${catkin_LIBRARIES}
)

target_link_libraries(
  action_demo_server
  ${catkin_LIBRARIES}
)

编译运行

^Cbanker@banker:~$ rosrun action_demo action_demo_server 
[ INFO] [1565945494.394334604]:  action server start.
[ INFO] [1565945497.440212600]: action_demo: Executing, creating actiondemo  order 5 with feedback_msg 0
[ INFO] [1565945497.440259549]: 1: feedback_.feedback_msg
[ INFO] [1565945498.440326674]: 2: feedback_.feedback_msg
[ INFO] [1565945499.440314090]: 3: feedback_.feedback_msg
[ INFO] [1565945500.440317296]: 4: feedback_.feedback_msg
[ INFO] [1565945501.440296284]: 5: feedback_.feedback_msg
[ INFO] [1565945502.440289997]: action_demo: Succeeded







banker@banker:~$ rosrun action_demo action_demo_client 
[ INFO] [1565945497.254264243]: Waiting for action server to start.
[ INFO] [1565945497.439740210]: Action server started, sending goal.
[ INFO] [1565945497.440392219]: Goal just went active
[ INFO] [1565945497.440531685]:  feedback_msg : 1 
[ INFO] [1565945498.440599365]:  feedback_msg : 2 
[ INFO] [1565945499.440703451]:  feedback_msg : 3 
[ INFO] [1565945500.440653442]:  feedback_msg : 4 
[ INFO] [1565945501.440654238]:  feedback_msg : 5 
[ INFO] [1565945502.440648618]: doneCb ,action finsh , result = 15

代码主要是客户端给服务器发送一个值（goal），服务端接收到后把1到这个值累加，每次累加会给客户端返回一个feedback_msg,完成后result的值就是累加的值

finshed！