ros_actionlib_tutorial 入门

actionlib 是ros中又一种通信机制，为了解决大型程序中服务机制响应阻塞时间过长的问题。
相比服务疑问一答的形式，多了一个反馈，不断反馈项目的进度。但有时候呢，如果这个service需要稍长一些的时间来运算之后才能反馈所需的结果，但在运行过程中，用户sonictl也许要根据运行的不同情况来决定是否取消request或者是需要周期性地获知运行状态。The actionlib 包就是提供这些工具，使得可以创建一些servers, 这些servers是用来执行需要长时间的任务的，并且是可以被preempted（预先制止）的。同时，actionlib包还提供了一个client接口，用来给server发请求。
官方wiki tutorial website:http://wiki.ros.org/actionlib_tutorials
本文内容记录与教程一致。
mkdir -p action_ws/src
cd action_ws
catkin_make && . devel/setup.bash
cd src
catkin_create_pkg actionlib_tutorial actionlib message_generation roscpp std_msgs actionlib_msgs
和话题/服务类似，actionlib需要建立自己的消息格式。后缀为.action
mkdir action && cd action
vim Fibonaccia.action(类似自定义的话题消息类型开头要大写)

#goal definition
int32 order
---
#result definition
int32[] sequence
---
#feedback
int32[] sequence

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(actionlib_tutorials)

find_package(catkin REQUIRED COMPONENTS  actionlib  message_generation  roscpp)
#利用add_action_files宏，声明 你想要生成的action
add_action_files(DIRECTORY action   FILES Fibonacci.action)#这句要在下句前面

generate_messages(DEPENDENCIES actionlib_msgs std_msgs)

#define catkin exports
catkin_package(
  INCLUDE_DIRS include 
  LIBRARIES actionlib_tutorials
  CATKIN_DEPENDS  actionlib  message_generation roscpp 
  DEPENDS system_lib 
)

 #add_executable(${PROJECT_NAME}_node src/actionlib_tutorials_node.cpp)

 #target_link_libraries(${PROJECT_NAME}_node   ${catkin_LIBRARIES} )

package.xml

<?xml version="1.0"?>
<package>
  <name>actionlib_tutorials</name>
  <version>0.0.0</version>
  <description>The actionlib_tutorials package</description>
  <maintainer email="robot@todo.todo">robot</maintainer>
  <license>BSD</license>
  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>action_msgs</build_depend>
  <build_depend>actionlib</build_depend>
  <build_depend>message_generation</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>rospy</build_depend>
  <build_depend>std_msgs</build_depend>
  <run_depend>action_msgs</run_depend>
  <run_depend>actionlib</run_depend>
  <run_depend>roscpp</run_depend>
  <run_depend>rospy</run_depend>
  <run_depend>std_msgs</run_depend>
  <run_depend>message_generation</run_depend> #这个是后续加上的，必须


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>

在工作空间编译catkin_make

$ ls devel/share/actionlib_tutorials/msg/    #生成的消息文件格式为.msg
FibonacciActionFeedback.msg  FibonacciAction.msg        FibonacciFeedback.msg
FibonacciResult.msg          FibonacciActionGoal.msg    FibonacciActionResult.msg  FibonacciGoal.msg
$ ls devel/include/actionlib_tutorials/ #src/include 里面没有头文件，在devel/下
FibonacciActionFeedback.h  FibonacciAction.h        FibonacciFeedback.h  FibonacciResult.h
FibonacciActionGoal.h      FibonacciActionResult.h  FibonacciGoal.h

现在在devel下居然还有一个share文件夹
一共生成6个头文件，三个为acition文件同名+goal/result/feedback
其他三个为aciton文件同名后+Action+goal/result/feedback(Action is apended)
These messages are then used internally by actionlib to communicate between the ActionClient and ActionServer;are generated by genaction.py:

头文件会用到的是FibonacciAction.h
不带Action的Goal Result Feedback 定义了基本的数据类型
fibonacci_server.cpp

#include "ros/ros.h"
#include <actionlib/server/simple_action_server.h>
#include <actionlib_tutorials/FibonacciAction.h>

class FibonacciAction
{
protected:
ros::NodeHandle nh_;
actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_;
std::string action_name_;
actionlib_tutorials::FibonacciFeedback feedback_;
actionlib_tutorials::FibonacciResult result_;

public:
FibonacciAction(std::string name):as_(nh_,name,boost::bind(&FibonacciAction::executeCB,this,_1),false), action_name_(name)
{
as_.start();
}
//靠字符串名字用来识别对应的服务器
~FibonacciAction(void){}
//ConstPtr 写法是boost的共享指针
void executeCB(const actionlib_tutorials::FibonacciGoal::ConstPtr &goal)
{
ros::Rate r(1);
bool success = true;
//// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);

ROS_INFO("%s:Executing,creating fibonacci sequence of order %i with seeds %i, %i",action_name_.c_str(),goal->order,feedback_.sequence[0],feedback_.sequence[1]);

//start
for(int i=1;i<=goal->order;i++)
{
// check that preempt has not been requested by the client
//if代表了action可以被中途取消的功能
if(as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
//set the action state to preempted 
as_.setPreempted();
success = false;
break;
}
feedback_.sequence.push_back(feedback_.sequence[i]+ feedback_.sequence[i-1]);
//publish the feedback
//没有结束前，周期提供feedback,最后执行成功再向客户端提供result(由feedback赋值)
as_.publishFeedback(feedback_);
//this sleeep is not necessary, the sequence is computed at 1hz for demonstration purpose
r.sleep();
}
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
//set the action states to succeeded
as_.setSucceeded(result_);
}
}

};

int main(int argc, char** argv)
{
    ros::init(argc,argv,"FIBONACCI");
    FibonacciAction fibonacci("fibonacci");
    ros::spin();

    return 0;
}

---

fibonacci_client.cpp

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <actionlib_tutorials/FibonacciAction.h>

int main(int argc,char** argv)
{
    ros::init(argc,argv,"test_fibonacci");
    //create action client
    //true casuses the client to spin its own thread
    //不需要nodehandle?
    actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci",true);
//ac初始化的字符串名字不可以改，与之前对应
    ROS_INFO("Waiting for action server to start");
    //wait for action server to start
    ac.waitForServer();

    ROS_INFO("Action server started");

    //sent a goal to the action
    //自定义的消息格式不含action~！
    actionlib_tutorials::FibonacciGoal goal;
    goal.order = 30;
    ac.sendGoal(goal);

    //wait for the action to return

    bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
    if(finished_before_timeout)
    {
    actionlib::SimpleClientGoalState state = ac.getState();
    ROS_INFO("Action finished:%s",state.toString().c_str());
    }
    else
        ROS_INFO("Action didnot finish before the time out");

    return 0;
}

---

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(actionlib_tutorials)


find_package(catkin REQUIRED COMPONENTS
  actionlib
  actionlib_msgs
  message_generation
  roscpp
  rospy
  std_msgs
)


 add_action_files(
   FILES
   Fibonacci.action
 )
generate_messages(
  DEPENDENCIES actionlib_msgs std_msgs  # Or other packages containing msgs
)
catkin_package(
  INCLUDE_DIRS include
  LIBRARIES actionlib_tutorials
  CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs
  DEPENDS system_lib
)


include_directories( include  ${catkin_INCLUDE_DIRS})



add_executable(fibonacci_server src/fibonacci_server.cpp)
target_link_libraries(fibonacci_server  ${catkin_LIBRARIES})
#add_dependencies可省，如下为默认写法
add_dependencies(fibonacci_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

add_executable(fibonacci_client src/fibonacci_client.cpp)
target_link_libraries(fibonacci_client  ${catkin_LIBRARIES})
add_dependencies(fibonacci_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

python version:
server.py

#!/usr/bin/env python
import rospy
import actionlib
import actionlib_tutorials.msg
#from actionlib_tutorials import *

class FibonacciAction(object):
    '''
    new tyle of class
    python2.4-3.0
    '''
    # # create messages that are used to publish feedback/result
    _feedback = actionlib_tutorials.msg.FibonacciFeedback()
    _result = actionlib_tutorials.msg.FibonacciResult()

    def __init__(self,name):
        self._action_name = name
        self._as = actionlib.SimpleActionServer(self._action_name,actionlib_tutorials.msg.FibonacciAction,execute_cb = self.execute_cb,auto_start = False)
        self._as.start()

    def execute_cb(self,goal):
        r = rospy.Rate(1)
        success = True

        self._feedback.sequence = []
        self._feedback.sequence.append(0)
        self._feedback.sequence.append(1)

        rospy.loginfo('%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i' % (self._action_name, goal.order, self._feedback.sequence[0], self._feedback.sequence[1]))

        for i in range(1,goal.order):
            if self._as.is_preempt_requested():
                rospy.loginfo('%s: Preempted' % self._action_name)
                self._as.set_preempted()
                success = False
                break
            self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
            self._as.publish_feedback(self._feedback)

            r.sleep()

        if success:
            self._result.sequence = self._feedback.sequence
            rospy.loginfo('%s: Succeeded' % self._action_name)
            self._as.set_succeeded(self._result)

if __name__ == '__main__':
    rospy.init_node('fibonacci')
    server = FibonacciAction(rospy.get_name())
    rospy.spin()

client.py

#!/usr/bin/env python
from __future__ import print_function
import rospy
import actionlib
import actionlib_tutorials.msg

def fibonacci_client():
    client = actionlib.SimpleActionClient('fibonacci',actionlib_tutorials.msg.FibonacciAction)
    client.wait_for_server()

    goal = actionlib_tutorials.msg.FibonacciGoal(order = 20)

    client.send_goal(goal)

    client.wait_for_result()

    return client.get_result()

if __name__ == '__main__':
    try:
        rospy.init_node('fibonacci_client_py')
        result = fibonacci_client()

        print("Result:", ', '.join([str(n) for n in result.sequence]))
    except rospy.ROSInterruptException:
        print("program interrupted before completion", file = sys.stderr)