ROS Action通信

应用场景：需要使用服务通信，但需要及时进行反馈。

在ROS中提供了actionlib功能包集，用于实现 action 通信。action 是一种类似于服务通信的实现，其实现模型也包含请求和响应，但是不同的是，在请求和响应的过程中，服务端还可以连续的反馈当前任务进度，客户端可以接收连续反馈并且还可以取消任务。

action通信接口

• goal:目标任务
• cacel:取消任务
• status:服务端状态
• result:最终执行结果(只会发布一次)
• feedback:连续反馈(可以发布多次)

自定义action文件(类似msg和service)

服务端 action01_server.cpp

/* 需求: 创建两个ROS节点，服务器和客户端， 客户端可以向服务器发送目标数据N（一个整型数据） 服务器会计算1到N之间所有整数的和，这是一个循环累加的过程，返回给客户端， 这是基于请求响应模式的， 又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s， 为了良好的用户体验，需要服务器在计算过程中， 每累加一次，就给客户端响应一次百分比格式的执行进度，使用action实现。 流程: 1.包含头文件; 2.初始化ROS节点; 3.创建 NodeHandle; 4.创建 action 服务对象; 5.处理请求(a. 解析提交的目标值；b. 产生连续反馈；c. 最终结果响应)产生反馈与响应; 回调函数实现 6.spin()回旋 */
#include"ros/ros.h"
#include"actionlib/server/simple_action_server.h"
#include"demo01_action/AddIntsAction.h"

// 重命名
typedef actionlib::SimpleActionServer<demo01_action::AddIntsAction> Server;

// 5.处理请求(a. 解析提交的目标值；b. 产生连续反馈；c. 最终结果响应)——回调函数实现 客户端提供的goal 和 服务端的server对象
void cb(const demo01_action::AddIntsGoalConstPtr &goalPtr,Server* server){
    
    // a. 解析提交的目标值
    int goal_num = goalPtr->num;
    ROS_INFO("客户端提交的目标值是:%d",goal_num);
    // b. 产生连续反馈 累加
    ros::Rate rate(10);// 10Hz
    int result = 0;
    ROS_INFO("开始连续反馈.....");
    for(int i = 1; i <= goal_num; i++)
    {
    
        // 累加
        result += i;
        // 休眠
        rate.sleep();
        // 产生连续反馈
        // void publishFeedback(const demo01_action::AddIntsFeedback &feedback)
        demo01_action::AddIntsFeedback fb;
        fb.progress_bar = i / (double)goal_num;
        server->publishFeedback(fb);
    }
    ROS_INFO("最终响应结果:%d",result);
    // c. 最终结果响应
    demo01_action::AddIntsResult r;
    r.result = result;
    server->setSucceeded(r);
}
int main(int argc, char *argv[])
{
    
    // 2.初始化ROS节点;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"addInts_server");// 节点
    // 3.创建 NodeHandle;
    ros::NodeHandle nh;
    // 4.创建 action 服务对象;
    /* SimpleActionServer(ros::NodeHandle n, NodeHandle std::string name, 话题名称 boost::function<void (const demo01_action::AddIntsGoalConstPtr &)> execute_callback, 回调函数 bool auto_start 是否自动启动 参数1: NodeHandle 参数2: 话题名称 参数3: 回调函数 参数4: 是否自动启动 */
    Server server(nh,"addInts",boost::bind(&cb,_1,&server),false); // 话题,_1是用来占位，传入的是回调函数goal的参数
    server.start();// 如果 atuto_start 为false, 那么需要手动调用该函数启动服务
    ROS_INFO("服务启动......");
    // 6.spin()回旋
    ros::spin();
    return 0;
}

客户端 action02_client.cpp

#include "ros/ros.h"
// 创建客户端对象
#include "actionlib/client/simple_action_client.h"
#include "demo01_action/AddIntsAction.h"

/* 需求: 创建两个ROS节点，服务器和客户端， 客户端可以向服务器发送目标数据N（一个整型数据） 服务器会计算1到N之间所有整数的和，这是一个循环累加的过程，返回给客户端， 这是基于请求响应模式的， 又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s， 为了良好的用户体验，需要服务器在计算过程中， 每累加一次，就给客户端响应一次百分比格式的执行进度，使用action实现。 流程: 1.包含头文件; 2.初始化ROS节点; 3.创建 NodeHandle; 4.创建 action 客户端对象; 5.发送请求 a. 连接建立: --- 回调函数 b. 处理连续反馈:--- 回调函数 c. 处理最终响应:--- 回调函数 6.spin(). */
typedef actionlib::SimpleActionClient<demo01_action::AddIntsAction> Client;


// 响应成功时的回调，处理最终结果(第3步)状态、结果诉讼诉讼
void done_cb(const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result){
    
    // 响应是否成功
    if (state.state_ == state.SUCCEEDED)
    {
    
        ROS_INFO("响应成功，最终结果 = %d",result->result);
    } else {
    
        ROS_INFO("请求失败！");
    }

}
// 激活回调，服务已经激活(第1步)
void active_cb(){
    
    ROS_INFO("客户端与服务端连接建立....");
}
// 连续反馈的回调，处理连续反馈(第2步)
void feedback_cb(const demo01_action::AddIntsFeedbackConstPtr &feedback){
    
    ROS_INFO("当前进度:%.2f",feedback->progress_bar);
}


int main(int argc, char *argv[])
{
    
    // 2.初始化ROS节点
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"addInts_client");
    // 3.创建 NodeHandle
    ros::NodeHandle nh;
    // 4.创建 action 客户端对象
    // SimpleActionClient(ros::NodeHandle & n, const std::string & name, bool spin_thread = true)
    actionlib::SimpleActionClient<demo01_action::AddIntsAction> client(nh,"addInts");// 话题
    // 5.发送请求
    // 注意: 判断服务器状态，等待服务启动
    ROS_INFO("等待服务器启动.....");
    client.waitForServer();
        // a. 连接建立: --- 回调函数
        // b. 处理连续反馈:--- 回调函数
        // c. 处理最终响应:--- 回调函数
    /* void sendGoal(const demo01_action::AddIntsGoal &goal, boost::function<void (const actionlib::SimpleClientGoalState &state, const demo01_action::AddIntsResultConstPtr &result)> done_cb,处理最终响应 boost::function<void ()> active_cb, 连接建立 boost::function<void (const demo01_action::AddIntsFeedbackConstPtr &feedback)> feedback_cb) 处理反馈 */
   // 参数1: 设置目标值
    demo01_action::AddIntsGoal goal;
    goal.num = 100;

    client.sendGoal(goal,&done_cb,&active_cb,&feedback_cb);
    // 6.spin().
    ros::spin();
    return 0;
}

服务端 action01_server_p.py

#! /usr/bin/env python

""" 需求: 创建两个ROS 节点，服务器和客户端， 客户端可以向服务器发送目标数据N(一个整型数据)服务器会计算 1 到 N 之间所有整数的和, 这是一个循环累加的过程，返回给客户端，这是基于请求响应模式的， 又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s， 为了良好的用户体验，需要服务器在计算过程中， 每累加一次，就给客户端响应一次百分比格式的执行进度，使用 action实现。 流程: 1.导包 2.初始化 ROS 节点 3.单独使用类封装， 4.类中创建 action 服务端对象 5.处理请求(a.解析目标值 b. 发送连续反馈 c. 响应最终结果)--- 回调函数 6.spin()回旋 """
import rospy
import actionlib
from demo01_action.msg import *

# 4.类中创建 action 服务端对象
# 5.处理请求(a.解析目标值 b. 发送连续反馈 c. 响应最终结果)--- 回调函数
class MyAction:
    def __init__(self):
        #SimpleActionServer(name, ActionSpec, execute_cb=None, auto_start=True)
        self.server = actionlib.SimpleActionServer("addInts",AddIntsAction,self.cb,False)
        self.server.start()
        rospy.loginfo("服务端启动.....")

    # 回调函数
    # 参数: 目标值
    def cb(self,goal):
        # a.解析目标值
        goal_num = goal.num
        rospy.loginfo("目标值:%d",goal_num)
        # b.循环累加，连续反馈
        rate = rospy.Rate(10)
        sum = 0 # 接受求和结果变量
        rospy.loginfo("请求处理中.....")
        for i in range(1,goal_num + 1):
            # 累加
            sum = sum + i
            rate.sleep()
            # 发送连续反馈
            fb_obj = AddIntsFeedback()
            fb_obj.progress_bar = i / goal_num
            self.server.publish_feedback(fb_obj)
        # c.响应最终结果
        rospy.loginfo("响应结果:%d",sum)
        result = AddIntsResult()
        result.result = sum        
        self.server.set_succeeded(result)

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("action_server_p")
    # 3.单独使用类封装
    myAction = MyAction()
    # 6.spin()回旋
    rospy.spin()

客户端 action02_client_p.py

#! /usr/bin/env python

""" 需求: 创建两个ROS 节点，服务器和客户端， 客户端可以向服务器发送目标数据N(一个整型数据)服务器会计算 1 到 N 之间所有整数的和, 这是一个循环累加的过程，返回给客户端，这是基于请求响应模式的， 又已知服务器从接收到请求到产生响应是一个耗时操作，每累加一次耗时0.1s， 为了良好的用户体验，需要服务器在计算过程中， 每累加一次，就给客户端响应一次百分比格式的执行进度，使用 action实现。 流程: 1.导包 2.初始化 ROS 节点 3.单独使用类封装， 4.类中创建 action 服务端对象 5.处理请求(a.解析目标值 b. 发送连续反馈 c. 响应最终结果)--- 回调函数 6.spin()回旋 """
import rospy
import actionlib
from demo01_action.msg import *

# 4.类中创建 action 服务端对象
# 5.处理请求(a.解析目标值 b. 发送连续反馈 c. 响应最终结果)--- 回调函数
class MyAction:
    def __init__(self):
        #SimpleActionServer(name, ActionSpec, execute_cb=None, auto_start=True)
        self.server = actionlib.SimpleActionServer("addInts",AddIntsAction,self.cb,False)
        self.server.start()
        rospy.loginfo("服务端启动.....")

    # 回调函数
    # 参数: 目标值
    def cb(self,goal):
        # a.解析目标值
        goal_num = goal.num
        rospy.loginfo("目标值:%d",goal_num)
        # b.循环累加，连续反馈
        rate = rospy.Rate(10)
        sum = 0 # 接受求和结果变量
        rospy.loginfo("请求处理中.....")
        for i in range(1,goal_num + 1):
            # 累加
            sum = sum + i
            rate.sleep()
            # 发送连续反馈
            fb_obj = AddIntsFeedback()
            fb_obj.progress_bar = i / goal_num
            self.server.publish_feedback(fb_obj)
        # c.响应最终结果
        rospy.loginfo("响应结果:%d",sum)
        result = AddIntsResult()
        result.result = sum        
        self.server.set_succeeded(result)

if __name__ == "__main__":
    # 2.初始化 ROS 节点
    rospy.init_node("action_server_p")
    # 3.单独使用类封装
    myAction = MyAction()
    # 6.spin()回旋
    rospy.spin()