ROS2 动作通信——自定义动作接口（Python）

实验目的：

         自定义动作接口，要求动作客户端发送一个整型数据给服务端，动作服务端进行累加求和，在累加过程中连续反馈进度，最后反馈结果给客户端。

1、创建工作空间和自定义接口功能包

        请参考：ROS2 话题通信——自定义消息类型（Python）-CSDN博客

2、自定义动作接口

        在interface_pkg功能包中新建action文件夹，用于存放动作接口文件，在action目录中创建Progress.action文件 

mkdir -p ./text_ws/src/interface_pkg/action
touch ./text_ws/src/interface_pkg/action/Progress.action

       在Progress.action文件定义以下数据格式 

int32 num
---
int32 sum
---
float64 progress

       修改interface_pkg中的CMakeLists.txt文件

        修改interface_pkg中的package.xml文件 

  <!--action构建工具依赖-->
  <buildtool_depend>rosidl_default_generators</buildtool_depend>
  <depend>action_msgs</depend>

         保存然后编译

cd text_ws/
colcon build --packages-select interface_pkg 

        在install目录下面生成了我们所需要的文件，即编译成功 

        创建动作功能包action_pkg，新建动作服务端和动作客户端节点

cd ./text_ws/src/
ros2 pkg create action_pkg --build-type ament_python --dependencies rclpy interface_pkg --node-name action_server
touch ./action_pkg/action_pkg/action_client.py

        编写动作服务端节点

import time

import rclpy                                        #ROS2 Python接口库
from rclpy.node import Node                         #ROS2 节点类
from rclpy.action import ActionServer               #ROS2动作服务器类
from interface_pkg.action import Progress           #自定义动作接口


class ProgressActionServer(Node):
    def __init__(self,name):
        super().__init__(name) 
        self.get_logger().info("动作服务端！")
        #创建动作服务端对象
        self._action_server = ActionServer(         #创建动作服务器（self、接口类型、动作名、回调函数）
            self,
            Progress,
            'get_sum',
            self.execute_callback)

    def execute_callback(self,goal_handle):             #执行收到动作目标之后的处理函数
        #首先获取目标值
        num = goal_handle.request.num
        sum = 0
        for i in range(1,num+1):#遍历累加，进行进度计算，并作为反馈发布
            sum += i
            feedback = Progress.Feedback()
            feedback.progress = i/num
            goal_handle.publish_feedback(feedback)
            self.get_logger().info("连续反馈:%.2f" % feedback.progress)
            time.sleep(1)

        #响应最终结果
        goal_handle.succeed()
        result = Progress.Result()
        result.sum = sum

        self.get_logger().info("计算结果：%d" % result.sum)
        return result
    
def main(args=None):                                        #ROS2节点主入口main函数
    rclpy.init(args=args)                                   #ROS2 Python接口初始化
    node = ProgressActionServer("action_server")            #创建ROS2节点对象并进行初始化
    rclpy.spin(node)                                        #循环等待ROS2退出
    node.destroy_node()                                     #销毁节点对象
    rclpy.shutdown()                                        #关闭ROS2 Python接口

        编写动作客户端节点

import rclpy                                        #ROS2 Python接口库
from rclpy.node import Node                         #ROS2 节点类
from rclpy.action import ActionClient               #ROS2动作客户端类
from interface_pkg.action import Progress           #自定义动作接口

import sys
from rclpy.logging import get_logger

class ProgressActionClient(Node):
    def __init__(self,name):
        super().__init__(name) 
        self.get_logger().info("动作客户端！")
        self._action_client = ActionClient(self,Progress,'get_sum')#创建动作客户端（self、接口类型、动作名）
    
    #发送动作目标的函数
    def send_goal(self,num):                  
        #连接服务端
        self._action_client.wait_for_server()
        #发送请求
        goal = Progress.Goal()
        goal.num = num
        self.future = self._action_client.send_goal_async(goal , self.feedback_callback)#(目标值，连续反馈回调函数)
        self.future.add_done_callback(self.goal_response_callback)#反馈完之后的回调函数执行

    #服务器收到目标之后反馈时的回调函数
    def goal_response_callback(self,future):
        goal_handle = future.result()       #接收动作的结果
        if not goal_handle.accepted:        #如果动作被拒执行
            self.get_logger().error('目标被拒绝！')      #输出日志信息
            return
        
        self.get_logger().info('目标被接收，正在处理中')   #输出日志信息，动作顺利执行

        #处理最终响应结果
        self.result_future = goal_handle.get_result_async()           #异步获取动作最终执行的结果反馈
        self.result_future.add_done_callback(self.get_result_callback)#设置一个收到动作最终结果的回调函数

    #动作最终结果的回调函数
    def get_result_callback(self,future):                          
        result = future.result().result                             #读取动作执行的结果
        self.get_logger().info('最终结果: {%d}' % result.sum)        #输出日志信息结果

    #连续反馈的回调函数
    def feedback_callback(self,feedback_msg):                      
        progress = feedback_msg.feedback.progress                  #读取反馈数据
        self.get_logger().info('连续反馈数据: {%.2f}' % progress)      #输出日志信息




def main(args=None):                                        #ROS2节点主入口main函数
    #动态解析传入的参数
    if len(sys.argv) !=2:
        get_logger("rclpy").error("请提交一个整型数据！")

    rclpy.init(args=args)                                   #ROS2 Python接口初始化
    node = ProgressActionClient("action_client")            #创建ROS2节点对象并进行初始化
    node.send_goal(int(sys.argv[1]))                        #发送动作目标
    rclpy.spin(node)                                        #循环等待ROS2退出
    node.destroy_node()                                     #销毁节点对象
    rclpy.shutdown()                                        #关闭ROS2 Python接口

        修改setup.py

        分别开两个终端先编译一下，然后运行服务端和客户端节点

colcon build --packages-select action_pkg
source install/setup.bash 
ros2 run action_pkg action_server
 
 
source install/setup.bash 
ros2 run action_pkg action_client 10

       测试成功。