动作服务器与客户端节点通讯 - (ROS学习笔记6)

       动作与话题和服务不同，在异步、双向，以及请求和响应之间需要很长的时间的情况，以及需要中途结果值等需要更复杂的编程的情况中显的格外有用。

生成功能包

       以下命令创建ros_tutorials_action功能包。这个功能包依赖于message_generation、std_msgs、actionlib_msgs、actionlib和roscpp功能包，因此将这些作为了依赖选项。

$ cd ~/catkin_ws/src

$ catkin_create_pkg ros_tutorials_action message_generation std_msgs actionlib_msgs actionlib roscpp

修改功能包配置文件（package.xml）

修改版本号。

修改构建配置文件（CMakeLists.txt）

       与《学习笔记5》介绍的ros_tutorials_topic和ros_tutorials_service节点的构建配置文件不同的是，如果这些节点的构建过程中生成了msg和srv文件，那么ros_tutorials_action功能包会生成动作文件（*.action）。此外，还添加了一个新的动作服务器节点和一个动作客户端节点作为使用它的示例节点。另外，由于我们使用了一个ROS之外的名为Boost的库，所以多了一个单独的依赖项选项。

$sudo gedit CMakeLists.txt 

修改内容如下：

find_package(Boost REQUIRED COMPONENTS system)
add_action_files(
  FILES
  Fibonacci.action
#   Action1.action
#   Action2.action
)
generate_messages(
  DEPENDENCIES
  actionlib_msgs
  std_msgs
)
catkin_package(
   LIBRARIES ros_tutorials_action
   CATKIN_DEPENDS std_msgs actionlib_msgs actionlib roscpp
   DEPENDS Boost
)
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
)
add_executable(action_server src/action_server.cpp)
add_executable(action_client src/action_client.cpp)
add_dependencies(action_server ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(action_client ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(action_server ${catkin_LIBRARIES})
target_link_libraries(action_client ${catkin_LIBRARIES})

修改完之后，保存退出。

创建动作文件

在CMakeLists.txt文件中加了如下选项。

add_action_files(FILES Fibonacci.action)

       这意味着在构建时包含服务Fibonacci.action，这个服务将用于本次的节点中。由于我们目前还没有创建Fibonacci.action，因此我们按以下顺序创建它。

$ roscd ros_tutorials_action  → 移动到功能包目录

$ mkdir action             → 在功能包中创建一个名为action的动作目录

$ cd action                → 移至创建的action目录

$ gedit Fibonacci.action      → 创建Fibonacci.action文件并修改内容 

       在动作文件中，三个连字符（---）用作分隔符，第一个是goal消息，第二个是result消息，第三个是feedback消息。goal消息和result消息之间的关系与上述srv文件相同，但主要区别在于feedback消息用于指定进程执行过程中的中间值传输。

Fibonacci.action文件中添加如下代码：

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

动作的5种基本消息

       除了可以在动作文件中找到的目标（goal）、结果（result）和反馈（feedback）之外，动作基本上还使用两个额外的消息：取消（cancel）和状态（status）。取消（cancel）消息使用actionlib_msgs/GoalID，它在动作运行时可以取消动作客户端和单独节点上的动作的执行。状态（status）消息可以根据状态转换 11 （如PENDING、ACTIVE、PREEMPTED和SUCCEEDED 12 ）检查当前动作的状态。

创建动作服务器节点

在CMakeLists.txt文件中如下添加了生成可执行文件的选项。

add_executable(action_server src/action_server.cpp)

       也就是说，是通过构建action_server.cpp文件来创建action_server可执行文件。我

们按以下顺序编写一个执行Action Server节点的功能的程序吧。

$ roscd ros_tutorials_action/src    → 移动到功能包的源代码目录src

$ sudo gedit action_server.cpp     → 创建和编辑源代码文件

添加如下代码

// ROS的基本头文件
#include <ros/ros.h>

// 动作库头文件
#include <actionlib/server/simple_action_server.h>

// FibonacciAction动作头文件（生成后自动生成）
#include <ros_tutorials_action/FibonacciAction.h>

class FibonacciAction
{
    protected:
    // 声明节点句柄
    ros::NodeHandle nh_;

    // 声明动作服务器
    actionlib::SimpleActionServer<ros_tutorials_action::FibonacciAction> as_;

    // 用作动作名称
    std::string action_name_;

    // 声明用于发布的反馈及结果
    ros_tutorials_action::FibonacciFeedback feedback_;
    ros_tutorials_action::FibonacciResult result_;

    public:

    // 初始化动作服务器（节点句柄、动作名称、动作后台函数）
    FibonacciAction(std::string name) :
    as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
    action_name_(name)
    {
        as_.start();
    }
    ~FibonacciAction(void)
    {
    }

    // 接收动作目标（goal）消息并执行指定动作（此处为斐波那契数列）的函数。
    void executeCB(const ros_tutorials_action::FibonacciGoalConstPtr &goal)
    {
        // 循环周期：1 Hz
        ros::Rate r(1);

        // 用作保存动作的成功或失败的变量    
        bool success = true;

        // 斐波那契数列的初始化设置，也添加了反馈的第一个（0）和第二个消息（1）
        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]);
        // 动作细节
        for(int i=1; i<=goal->order; i++)
        {
            // 从动作客户端得知动作取消
            if (as_.isPreemptRequested() || !ros::ok())
            {
                // 通知动作取消
                ROS_INFO("%s: Preempted", action_name_.c_str());
                // 取消动作
                as_.setPreempted();
                // 看作动作失败并保存到变量
                success = false;
                break;
            }

            // 除非有动作取消或已达成动作目标
            // 将当前斐波纳契数字加上前一个数字的值保存到反馈值。
            feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);

            // 发布反馈。
            as_.publishFeedback(feedback_);

            // 按照上面定义的循环周期调用暂歇函数。
            r.sleep();
        }

        // 如果达到动作目标值，则将当前斐波那契数列作为结果值传输。
        if(success)
        {
            result_.sequence = feedback_.sequence;
            ROS_INFO("%s: Succeeded", action_name_.c_str());
            as_.setSucceeded(result_);
        }
    }
};

// 节点主函数
int main(int argc, char** argv)
{
    // 初始化节点名称
    ros::init(argc, argv, "action_server");

    // 声明Fibonacci （动作名: ros_tutorial_action）
    FibonacciAction fibonacci("ros_tutorial_action");

    // 等待动作目标
    ros::spin();
    return 0;
}

然后保存退出。

创建动作客户端节点

       与动作服务器节点一样，客户端节点的设置内容也作为选项加到了CMakeLists.txt文件中。

add_executable(action_client src/action_client.cpp)

       也就是说，通过构建一个名为action_client.cpp的文件来创建action_client可执行文件。我们按照以下顺序编写一个执行动作客户端节点功能的程序。

$ roscd ros_tutorials_action/src    → 移动到功能包的源代码目录src

$ sudo gedit action_client.cpp     → 创建并修改源代码文件

添加如下代码：

// ROS的基本头文件
#include <ros/ros.h>

// 动作库头文件
#include <actionlib/client/simple_action_client.h>

// 动作目标状态头文件
#include <actionlib/client/terminal_state.h>

// FibonacciAction动作头文件（构建后自动生成）
#include <ros_tutorials_action/FibonacciAction.h>

// 节点主函数
int main (int argc, char **argv)
{
    // 初始化节点名称
    ros::init(argc, argv, "action_client");

    // 声明动作客户端（动作名称：ros_tutorial_action）
    actionlib::SimpleActionClient<ros_tutorials_action::FibonacciAction> ac("ros_tutorial_action", true);
    ROS_INFO("Waiting for action server to start.");

    // 等待动作服务器启动
    ac.waitForServer();
    ROS_INFO("Action server started, sending goal.");

    // 声明动作目标
    ros_tutorials_action::FibonacciGoal goal;

    // 指定动作目标（进行20次斐波那契运算）
    goal.order = 20;

    // 发送动作目标
    ac.sendGoal(goal);

    // 设置动作完成时间限制（这里设置为30秒）
    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;
}

然后保存退出。

构建节点

$ cd ~/catkin_ws && catkin_make      → 转到catkin目录并运行catkin构建

运行测试

         运行动作服务器

        我们之前编写的动作服务器在指定动作目标（goal）前不做任何行动，只会等待。因此，执行以下命令时，动作服务器会等待来自动作客户端的目标（goal）指定。运行节点之前不要忘记运行roscore。

$ roscore

$ rosrun ros_tutorials_action action_server

       动作中的动作目标（goal）和结果（result）的用法与服务中的请求和响应类似，这是动作和服务的相似之处。但不同之处在于，动作还有作为中途值的反馈（feedback）消息。这与服务类似，但其消息的实际通信方式与话题非常类似。因此，可以通过rqt_graph和rostopic list命令来查看当前动作消息的使用情况。

$ rostopic list

/ros_tutorial_action/cancel

/ros_tutorial_action/feedback

/ros_tutorial_action/goal

/ros_tutorial_action/result

/ros_tutorial_action/status

/rosout

/rosout_agg

如果想了解更多有关每条消息的信息，请将-v选项添加到rostopic list中。这将单独显示发布和订阅的话题，如下所示：

$ rostopic list -v

       为了查看可视化信息，请使用以下命令。  动作消息、动作服务器和客户端之间的关系如图7-8所示，是双向发送和接收。在这里，动作信息由名字ros_tutorial_action/action_topics统一表示，当关闭菜单中的Actions时，可以看到所有5个消息，如图所示。在这里我们可以看到，这个动作由5个话题以及发布和订阅这些话题的节点组成。

运行动作客户端

       使用以下命令运行动作客户端。在动作客户端启动的同时，会通过动作目标消息将动作目标设为20。

$ rosrun ros_tutorials_action action_client