ROS actionlib学习（一）

　　actionlib是ROS中一个很重要的功能包集合，尽管在ROS中已经提供了srevice机制来满足请求—响应式的使用场景，但是假如某个请求执行时间很长，在此期间用户想查看执行的进度或者取消这个请求的话，service机制就不能满足了，但是actionlib可满足用户这种需求。例如，控制机器人运动到地图中某一目标位置，这个过程可能复杂而漫长，执行过程中还可能强制中断或反馈信息，这时actionlib就能大展伸手了。

actionlib使用client-server工作模式，ActionClient 和ActionServer通过"ROS Action Protocol"进行通信，"ROS Action Protocol"以ROS消息方式进行传输。此外ActionClient 和ActionServer给用户提供了一些简单的接口，用户使用这些接口可以完成goal请求（client-side）和goal执行（server-side）。

　　ActionClient 和ActionServer之间使用action protocol通信，action protocol就是预定义的一组ROS message，这些message被放到ROS topic上在 ActionClient 和ActionServer之间进行传实现二者的沟通。

　　ROS Messages：

goal - Used to send new goals to servers. 代表一个任务，可以被ActionClient发送到ActionServer。比如在MoveBase中，它的类型是PoseStamped，包含了机器人运动目标位置的信息。
cancel - Used to send cancel requests to servers
status - Used to notify clients on the current state of every goal in the system
feedback - Used to send clients periodic auxiliary information for a goal. 服务端定期告知Client当前Goal执行过程中的情况。在Move Base案例中，它表示机器人当前姿态。
result - Used to send clients one-time auxiliary information upon completion of a goal

　　ROS系统在action文件（文件名后缀为.action）中定义了上述goal、result、feedback等消息。The .action file has the goal definition, followed by the result definition, followed by the feedback definition, with each section separated by 3 hyphens (---).

　　下面是一个示意的例子，在./action/DoDishes.action文件中对洗碗这一任务进行定义：goal为使用某一洗碗机洗碗，result为总共洗好的碗数目，feedback为洗碗进度。

# 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

　　下面在catkin_ws/src目录下创建一个测试package：

catkin_create_pkg actionlib_test roscpp std_msgs actionlib actionlib_msgs message_generation rospy

　　在package的CMakeLists.txt文件中加入下面这几行：

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

　　注意如果使用catkin_create_pkg创建包时没有添加actionlib相关的依赖项，要将上面CMakeLists中第一行的注释去掉，另外还要在package.xml文件中加入下面几行。因为我们在创建包时就添加好了相关依赖，所以这一步骤可以省略。

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

　　使用catkin_make编译即可查看生成的消息文件，这些消息之后将会用于ActionClient 和 ActionServer间的通信。

　　另外可以看到，在devel/include/actionlib_test/中生成了相关的头文件：

C++ SimpleActionClient

　　client示例代码client.cpp如下，它会等待Server连接，发送Goal，获取状态。SimpleActionClient完整的API可以参考C++ SimpleActionClient

#include <actionlib_test/DoDishesAction.h> 
#include <actionlib/client/simple_action_client.h>

typedef actionlib::SimpleActionClient<actionlib_test::DoDishesAction> Client;

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

  Client client("do_dishes", true); // true -> don't need ros::spin()
  client.waitForServer(); // Waits for the ActionServer to connect to this client
  actionlib_test::DoDishesGoal goal;
  // Fill in goal here
  client.sendGoal(goal); // Sends a goal to the ActionServer
  client.waitForResult(ros::Duration(5.0)); // Blocks until this goal finishes
  if (client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    printf("Yay! The dishes are now clean\n");
  printf("Current State: %s\n", client.getState().toString().c_str());
  return 0;
}

C++ SimpleActionServer

　　服务端代码server.cpp如下，SimpleActionServert完整的API可以参考 C++ SimpleActionServer

#include <actionlib_test/DoDishesAction.h> 
#include <actionlib/server/simple_action_server.h>

typedef actionlib::SimpleActionServer<actionlib_test::DoDishesAction> Server;

void execute(const actionlib_test::DoDishesGoalConstPtr& goal, Server* as) 
{
  // Do lots of awesome groundbreaking robot stuff here
  as->setSucceeded();
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "do_dishes_server");
  ros::NodeHandle n;
  Server server(n, "do_dishes", boost::bind(&execute, _1, &server), false);
  server.start();
  ros::spin();
  return 0;
}

　　在CMakeLists文件中加入下面这几行：

add_executable(client   src/client.cpp)
add_executable(server   src/server.cpp)
target_link_libraries( client ${catkin_LIBRARIES})
target_link_libraries( server ${catkin_LIBRARIES})

　　使用catkin_make进行编译完成后输入指令rosrun actionlib_test server 运行server，通过rostopic list查看系统中的话题如下：

　　使用rqt_graph命令可以查看节点和消息的关系，可以看出server端会接收goal和cancel消息，并发出status、result、feedback消息：

　　接着输入指令rosrun actionlib_test client 运行client，结果如下图所示：

Python SimpleActionClient

　　除了C++也可以使用Python实现同样的功能，client.py如下（API可以参考Python SimpleActionClient）：

#! /usr/bin/env python

import roslib
roslib.load_manifest('actionlib_test')
import rospy
import actionlib

from actionlib_test.msg import DoDishesAction, DoDishesGoal

if __name__ == '__main__':
    rospy.init_node('do_dishes_client')
    client = actionlib.SimpleActionClient('do_dishes', DoDishesAction)
    client.wait_for_server()

    goal = DoDishesGoal()
    # Fill in the goal here
    client.send_goal(goal)
    client.wait_for_result(rospy.Duration.from_sec(5.0))

Python SimpleActionServer

　　server.py程序如下（API可参考Python SimpleActionServer）：

#! /usr/bin/env python

import roslib
roslib.load_manifest('actionlib_test')
import rospy
import actionlib

from actionlib_test.msg import DoDishesAction

class DoDishesServer:
  def __init__(self):
    self.server = actionlib.SimpleActionServer('do_dishes', DoDishesAction, self.execute, False)
    self.server.start()

  def execute(self, goal):
    # Do lots of awesome groundbreaking robot stuff here
    self.server.set_succeeded()


if __name__ == '__main__':
  rospy.init_node('do_dishes_server')
  server = DoDishesServer()
  rospy.spin()