机器人操作系统ROS（17）：actionlib进度查询

ROS 里除了基本的msg publisher/Subscriber和Server/Client之外，还有ActionLib 值得注意。
ROS中的服务service是一问一答的形式，你来查询了，我就返给你要的信息。
Actionlib也有服务的概念，但是它不一样的地方是：不是一问一答，而多了一个反馈，它会不断反馈项目进度。

actionlib提供动作（action ROS中表示一些长时间连续性的行为）的编程接口，提供动作执行时的查看状态，终止， 抢占等一系列功能。
如navigation下的move_base, 你设定了目标点，反馈信息可能是机器人在规划路径上的即时位姿，直到机器人到达目标点，返回SUCCEEDED消息。Examples of this include moving the base to a target location, performing a laser scan and returning the resulting point cloud, detecting the handle of a door, etc.

因为move_base会遇到很多有关actionlib的实现，需要先例解actionlib才可以清楚navigation的其他包。
move_base作为navigation的逻辑核心，实现了一个供上层节点调用的action 接口（通过actionlib 实现），即给定坐标信息与相应坐标位置，执行相应的安全导航动作。对于下层控制器，输出为cmd_vel 速度。 它规定了这个navigation的行为。
ROS中的自主移动规划层向上的编程接口一般都是通过这个actionlib。

1 ActionClient, ActionServer
In any large ROS based system, there are cases when someone would like to send a request to a node to perform some task, and also receive a reply to the request. This can currently be achieved via ROS services.

In some cases, however, if the service takes a long time to execute, the user might want the ability to cancel the request during execution or get periodic feedback about how the request is progressing. The actionlib package provides tools to create servers that execute long-running goals that can be preempted. It also provides a client interface in order to send requests to the server.

ActionClient 和 ActionServer 通过 ROS Action Protocol （ROS Action 协议，建立在ROS messages的基础上）来通信。
Client&Server 为用户提供了简单的API，通过函数的调用和回调，用来在client端request一个目标，或者，在server端来执行达成一个目标。下图说明这个机制如何运行：

这个是Client主动，ActionServer被动.

=== a Simple Action Server using the Execute Callback

2 Goal, Feedback, & Result
In order for the client and server to communicate, we need to define a few messages on which they communicate. This is with anaction specification. This defines the Goal, Feedback, and Result messages with which clients and servers communicate:

Goal
To accomplish tasks using actions, we introduce the notion of a goal that can be sent to an ActionServer by an ActionClient. In the case of moving the base, the goal would be a PoseStamped message that contains information about where the robot should move to in the world. For controlling the tilting laser scanner, the goal would contain the scan parameters (min angle, max angle, speed, etc).
Feedback
Feedback provides server implementers a way to tell an ActionClient about the incremental progress of a goal. For moving the base, this might be the robot’s current pose along the path. For controlling the tilting laser scanner, this might be the time left until the scan completes.
Result
A result is sent from the ActionServer to the ActionClient upon completion of the goal. This is different than feedback, since it is sent exactly once. This is extremely useful when the purpose of the action is to provide some sort of information. For move base, the result isn’t very important, but it might contain the final pose of the robot. For controlling the tilting laser scanner, the result might contain a point cloud generated from the requested scan.
.action File
The action specification is defined using a .action file. The .action file has the goal definition, followed by the result definition, followed by the feedback definition, with each section separated by 3 hyphens (—).
These files are placed in a package’s ./action directory, and look extremely similar to a service’s .srv file. An action specification for doing the dishes might look like the following:
./action/DoDishes.action
# Define the goal
uint32 dishwasher_id # Specify which dishwasher we want to use
—
# Define the result
uint32 total_dishes_cleaned
—
# Define a feedback message
float32 percent_complete
熟悉Message机制就知道，这个类似，就是定义 action specification的消息数据类型.
Based on this .action file, 6 messages need to be generated in order for the client and server to communicate. This generation can be automatically triggered during the make process.
For the DoDishes.action, the following messages are generated by genaction.py:
DoDishesAction.msg
DoDishesActionGoal.msg
DoDishesActionResult.msg
DoDishesActionFeedback.msg
DoDishesGoal.msg
DoDishesResult.msg
DoDishesFeedback.msg

3. Catkin, Rosbuild
3.1 Catkin
Add the following to your CMakeLists.txt file before catkin_package().
注意你要用 actionlib 的话，记得修改CMakeLists.txt .

find_package(catkin REQUIRED genmsg actionlib_msgs actionlib)
add_action_files(DIRECTORY action FILES DoDishes.action)
generate_messages(DEPENDENCIES actionlib_msgs)
actionlib
actionlib_msgs

3.2 Rosbuild

What following lists some tutorials examples.
4. create a test stack
$ cd ~/catkin_ws/src
$ catkin_create_pkg learning_actionlib actionlib message_generation roscpp rospy std_msgs actionlib_msgs

=== CH 5 – a Simple Action Server using the Execute Callback

5 C++ Example 1: Simple Action Server using the Execute Callback
This tutorial covers using the simple_action_server library to create a Fibonacci action server. This example action server generates a Fibonacci sequence,
the goal is the order of the sequence,
the feedback is the sequence as it is computed,
and the result is the final sequence.

5.1 Creating the Action Messages
Before writing an action it is important to define the goal, result, and feedback messages.
The action messages are generated automatically from the .action file.
This file defines the type and format of the goal, result, and feedback topics for the action.

mkdir ./action
vi ./action/fib.action

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

5.2 Change CMakeLists.txt
a few things need to be added to CMakeLists.txt, add the actionlib_msgs package to the find_package macro’s argument like this

vi CMakeLists.txt

**1** find_package macro:
find_package(catkin REQUIRED COMPONENTS actionlib_msgs)

**2**: add_action_files macro
add_action_files(
DIRECTORY action
FILES fib.action
)

**3** generate_messages macro:
generate_messages(
DEPENDENCIES actionlib_msgs std_msgs # Or other packages containing msgs
)

**4** catkin_package macro:
catkin_package(
CATKIN_DEPENDS actionlib_msgs
)

Note: Sometimes you have to setup your package.xml, since we are generating messages you have to declare on the manifest file that at run time you have to generate messages.

**5** You could just insert the follow line:
vi package.xml
actionlib_msgs
actionlib_msgs

**6**
Now, we can automatically generate msg files of your action files, and also see the result:
$ cd ~/catkin_ws
$ catkin_make
$ ls devel/share/mimebot/msg/
…
fibActionFeedback.msg fibActionGoal.msg fibAction.msg fibActionResult.msg fibFeedback.msg fibGoal.msg fibResult.msg

**7**
Also, we can To manually generate the message files from this file,

$ roscd mimebot
$ rosrun mimebot genaction.py -o msg/ action/fib.action
…
Generating for action fib

5.3 Writing a Simple Server

$ vi ./src/m_fib_server.cpp

#include
#include
// Include the action library used from implementing simple actions.
#include
// The file generated automatically from the /devel/share/mimebot/msg/fibAction.msg file .
// this action message generated from the fib.action file .

class fibAction
{
protected:
ros::NodeHandle nh_;
//The node handle is constructed and passed into the action server during construction of the action.

actionlib::SimpleActionServer as_;
//The action server is constructed in the constructor of the action
//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

mimebot::fibFeedback feedback_;
mimebot::fibResult result_;
//The feedback and result messages are created for publishing in the action.

public:
fibAction(std::string name) :
as_(nh_, name, boost::bind(&fibAction::executeCB, this, _1), false),
action_name_(name) { as_.start(); }
//In the action constructor, an action server is created.
//The action server takes arguments of a node handle, name of the action, and optionally an executeCB.

~fibAction(void)
{
}

//The callback function is passed a pointer to the goal message.
// Note: This is a boost shared pointer, given by appending “ConstPtr” to the end of the goal message type.
void executeCB(const mimebot::fibGoalConstPtr &goal)
{
// helper variables
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);

// publish info to the console for the user
ROS_INFO(“%s is executing, creating sequence of order %i with seeds %i, %i”, action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);

// start executing the action
for(int i=1; i<=goal->order; i++)
{
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO(“%s is 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
as_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep();
}

if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO(“%s is Succeeded”, action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}

};

int main(int argc, char** argv)
{
ros::init(argc, argv, “m_fib”);

fibAction fibonacci(“action_fib”);
ros::spin();

return 0;
}

5.4 change CMakeLists.txt

Add the following lines to your CMakeLists.txt file:

add_executable(m_fib_server src/m_fib_server.cpp)
target_link_libraries(m_fib_server ${catkin_LIBRARIES} )
add_dependencies(m_fib_server ${mimebot_EXPORTED_TARGETS})

$ catkin_make

5.5
test
$ rosrun mimebot m_fib_server
to cheak:
$ rostopic list -v
…
/fib/cancel
/fib/feedback
/fib/goal
/fib/result
/fib/status
or,
$ rqt_graph

=== CH 6 clinet

5.6 Writing Action Client

$ vi mimebot/src/m_fib_client.cpp

#include
#include
//the action library used from implementing simple action clients.
#include
//defines the possible goal states.
#include

int main (int argc, char **argv)
{
ros::init(argc, argv, “m_fib_client”);

// create the action client
// true causes the client to spin its own thread
actionlib::SimpleActionClient ac(“action_fib”, true);
//The action client constructor also takes two arguments, the server name to connect to and a boolean option to automatically spin a thread.
//Here the action client is constructed with the server name and the auto spin option set to 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
mimebot::fibGoal goal;
goal.order = 20;
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 did not finish before the time out.”);

//exit
return 0;
}

5.7 change CMakeLists.txt

add_executable(m_fib_client src/m_fib_client.cpp)

target_link_libraries( m_fib_client ${catkin_LIBRARIES} )
add_dependencies( m_fib_client ${mimebot_EXPORTED_TARGETS})

$catkin_make

5.8 to ckeak
$ rostopic echo /fib/feedback
$ rostopic echo /fib/result
$ rosrun learning_actionlib fibonacci_server
$ rosrun learning_actionlib fibonacci_client
…

6. Python Example2: Simple Action Server using the Execute Callback
6.1 Writing a Simple Action Server
#! /usr/bin/env python
import roslib; roslib.load_manifest(‘actionlib_tutorials’)
import rospy
import actionlib
import actionlib_tutorials.msg

class FibonacciAction(object):
# 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)
#Here, the SimpleActionServer is created, we pass it a name, an action type, and optionally an execute callback
#Since we’ve specified an execute callback in this example, a thread will be spun for us which allows us to take long running actions in a callback received when a new goal comes in.
self._as.start()

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

# append the seeds for the fibonacci sequence
self._feedback.sequence = []
self._feedback.sequence.append(0)
self._feedback.sequence.append(1)

# publish info to the console for the user
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]))

# start executing the action
for i in xrange(1, goal.order):
# check that preempt has not been requested by the client
if self._as.is_preempt_requested():
rospy.loginfo(‘%s: Preempted’ % self._action_name)
self._as.set_preempted()
success = False
break
#An important component of an action server is the ability to allow an action client to request that the goal under execution be canceled. When a client requests that the current goal be preempted, the action server should cancel the goal, perform any necessary cleanup, and call the set_preempted function, which signals that the action has been preempted by user request. Here, we’ll check if we’ve been preempted every second. We could, alternatively, receive a callback when a preempt request is received.

self._feedback.sequence.append(self._feedback.sequence[i] + self._feedback.sequence[i-1])
# publish the feedback
self._as.publish_feedback(self._feedback)
# this step is not necessary, the sequence is computed at 1 Hz for demonstration purposes
r.sleep()
#Here, the Fibonacci sequence is put into the feedback variable and then published on the feedback channel provided by the action server. Then, the action continues looping and publishing feedback.

if success:
self._result.sequence = self._feedback.sequence
rospy.loginfo(‘%s: Succeeded’ % self._action_name)
self._as.set_succeeded(self._result)
#Once the action has finished computing the Fibonacci sequence, the action server notifies the action client that the goal is complete by calling set_succeeded.

if __name__ == ‘__main__’:
rospy.init_node(‘fibonacci’)
FibonacciAction(rospy.get_name())
rospy.spin()

6.2 Writing a Simple Action Client
#! /usr/bin/env python
import roslib; roslib.load_manifest(‘actionlib_tutorials’)
import rospy
# Brings in the SimpleActionClient
import actionlib
# Brings in the messages used by the fibonacci action, including the goal message and the result message.
import actionlib_tutorials.msg

def fibonacci_client():
# Creates the SimpleActionClient, passing the type of the action (FibonacciAction) to the constructor.
client = actionlib.SimpleActionClient(‘fibonacci’, actionlib_tutorials.msg.FibonacciAction)
#The action client and server communicate over a set of topics, described in the actionlib protocol. The action name describes the namespace containing these topics, and the action specification message describes what messages should be passed along these topics

# Waits until the action server has started up and started
# listening for goals.
client.wait_for_server()
#Sending goals before the action server comes up would be useless. This line waits until we are connected to the action server.

# Creates a goal to send to the action server.
goal = actionlib_tutorials.msg.FibonacciGoal(order=20)

# Sends the goal to the action server.
client.send_goal(goal)

# Waits for the server to finish performing the action.
client.wait_for_result()

# Prints out the result of executing the action
return client.get_result() # A FibonacciResult

if __name__ == ‘__main__’:
try:
# Initializes a rospy node so that the SimpleActionClient can
# publish and subscribe over ROS.
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”

6.3 to check
$ rosrun actionlib_tutorials fibonacci_server
$ rosrun actionlib_tutorials fibonacci_client.py
$ rostopic echo fibonacci/feedback

7. C++ Example3: Writing a Simple Action Server using the Goal Callback Method
This tutorial covers using the simple_action_server library to create an averaging action server. This example shows how to use an action to process or react to incoming data from ros nodes. The action server averages data from a ros node, the goal is the number of samples to average, the feedback is the sample number, the sample data, the current average, and current standard deviation, and the result is the average and standard deviation of the requested number of samples.

7.1 Create learning_actionlib/action/Averaging.action
#goal definition
int32 samples
—
#result definition
float32 mean
float32 std_dev
—
#feedback
int32 sample
float32 data
float32 mean
float32 std_dev

7.2 manually generate the message files from this file:
$ roscd learning_actionlib
$ rosrun actionlib_msgs genaction.py -o msg/ action/Averaging.action<—-???
or,
automatically generate the message files during the make process, need to add the following to CMakeLists.txt:
find_package(catkin REQUIRED COMPONENTS actionlib std_msgs message_generation)
add_action_files(DIRECTORY action FILES Averaging.action)
generate_messages(DEPENDENCIES std_msgs actionlib_msgs)
and
$ catkin_make.

7.3 nano learning_actionlib/src/averaging_server.cpp

7.4 Add the following line to CMakeLists.txt file:
add_executable(averaging_server src/averaging_server.cpp)
target_link_libraries(averaging_server ${catkin_LIBRARIES})

7.5 catkin_make
7.6 to cheak
$ rosrun learning_actionlib averaging_server

7.7 Writing a Threaded Simple Action Client
$ nano learning_actionlib/src/averaging_client.cpp

7.8 Add the following line to CMakeLists.txt file:
add_executable(averaging_client src/averaging_client.cpp)
target_link_libraries(averaging_client ${catkin_LIBRARIES})
catkin_make

7.9 to check
$ rosrun learning_actionlib averaging_server
$ rosrun learning_actionlib averaging_client

7.10 from other node

4. C++ samples
4.1 C++ ActionClient
4.2 C++ ActionServer
5. Python samples
5.1 Python ActionClient
5.2 Python ActionServer

http://www.cnblogs.com/feixiao5566/p/4757916.html