ROS笔记5--动作通讯

标签：goal get -- self 笔记 result action Progress ROS

1、动作通讯简介

机器人是一个复杂的智能系统，并不仅仅是键盘遥控运动、识别某个目标这么简单，我们需要实现的是送餐、送货、分拣等满足具体场景需求的机器人。在这些应用功能的实现中，另外一种ROS通信机制也会被常常用到——那就是动作。 从这个名字上就可以很好理解这个概念的含义，这种通信机制的目的就是便于对机器人某一完整行为的流程进行管理。 动作通讯是一种带有连续反馈的通讯模型，在通讯双方中，客户端发送请求数据到服务端，服务端响应结果给客户端，但是在服务端接收到请求产生最终响应的过程中，会发送连续的反馈信息给客户端。 客户端发送一个运动的目标，想让机器人动起来，服务器端收到之后，就开始控制机器人运动，一边运动，一边反馈当前的状态。 如果是一个导航动作，这个反馈可能是当前所处的坐标，如果是机械臂抓取，这个反馈可能又是机械臂的实时姿态。 动作通讯客户端/服务器模型如下：   功能如下：动作客户端提交一个整数类型num，动作服务器接收请求数据并累加1-num之间的所有整数，将最终结果返回给客户端，且每次累计计算后将当前结果返回给客户端

2、新建功能包

2.1、动作通讯需要先创建通讯接口

cd ~/ros_ws/src ros2 pkg create --build-type ament_cmake pkg_interfaces

2.2、接下来在pkg_interfaces功能包下创建一个action文件夹，并在action文件夹内新建【Progress.action】文件，内容如下：

int64 num 
--- 
int64 sum 
--- 
float64 progress

2.3、在package.xml中添加依赖包，内容如下：

<buildtool_depend>rosidl_default_generators</buildtool_depend>
<depend>action_msgs</depend>
<member_of_group>rosidl_interface_packages</member_of_group>

2.4、在CMakeLists.txt中添加如下配置：

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
    "action/Progress.action"

2.5、编译功能包

cd ~/ros_ws colcon build --packages-select pkg_interfaces

编译完成后在工作空间的install目录下将生成Progress.action文件对应的C++和Python文件，可以在终端进入工作空间，通过如下命令查看是否编译成功：

source install/setup.bash 
ros2 interface show pkg_interfaces/action/Progress

如果终端输出与Progress.action文件一致的内容则正常。

3、创建动作功能包

cd ~/ros_ws/src 
ros2 pkg create pkg_action --build_type ament_python --dependencies rclpy pkg_interfaces --node-name action_server_demo

4、服务端实现

import time
import rclpy
from rclpy.action import ActionServer
from rclpy.node import Node
from pkg_interfaces.action import Progress

class Action_Server(Node):
    def __init__(self):
        super().__init__('progress_action_server')
        # 创建动作服务端
        self._action_server = Action_Server(
            self, Progress, 'get_sum', self.execute_callback
        )
        self.get_logger().info("action service starting!!!")

    def execute_callback(self, goal_handle):
        self.get_logger().info('start execute task...')
        # 生成连续反馈
        feedback_msg = Progress.Feedback()
        sum = 0
        for i in range(1, goal_handle.request.num + 1):
            sum += i
            feedback_msg.progress = i / goal_handle.request.num
            self.get_logger().info('callback: %.2f' % feedback_msg.progress)
            goal_handle.publish_feedback(feedback_msg)
            time.sleep(1)
        
        # 生成最终响应
        goal_handle.succeed()
        result = Progress.Result()
        result.sum = sum
        self.get_logger().info("task done!!!")
        return result
    
def main(args=None):
    rclpy.init(args=args)
    Progress_action_server = Action_Server()
    rclpy.spin(Progress_action_server)
    Progress_action_server.destroy_node()
    rclpy.shutdown()

5、客户端实现

import rclpy
from rclpy.action import ActionClient
from rclpy.node import Node
from pkg_interfaces.action import Progress

class Action_Client(Node):
    def __init__(self):
        super().__init__('progress_action_client')
        # 创建动作客户端
        self._action_client = Action_Client(self, Progress, 'get_sum')

    def send_goal(self, num):
        # 发送请求
        goal_msg = Progress.Goal()
        goal_msg.num = num
        self._action_client.wait_for_server()
        self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)
        self._send_goal_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().info('request refuesd')
            return
        self.get_logger().info('request received, start execute task')
        self._get_result_future = goal_handle.get_result_async()
        self._get_result_future.add_done_callback(self._get_result_callback)
    
    # 处理最终响应
    def get_result_callback(self, future):
        result = future.result().result
        self.get_logger.info('final result sum = %d' % result.sum)
        rclpy.shutdown
    
    # 处理连续返回
    def feedback_callback(self, feedback_msg):
        feedback = (int)(feedback_msg.feedback.progress * 100)
        self.get_logger().info('now progress: %d%%' % feedback)

def main(args=None):
    rclpy.init(args=args)
    action_client = Action_Client()
    action_client.send_goal(10)
    rclpy.spin(action_client)

6、编辑配置文件、编译工作空间

 

cd ~/ros_ws 
colcon build --packages-select pkg_action source install/setup.bash

7、运行程序

分终端分别启动服务端和客户端

ros2 run pkg_action action_server_demo 
ros2 run pkg_action action_client_demo

   

标签：goal,get,--,self,笔记,result,action,Progress,ROS
From： https://www.cnblogs.com/jackion5/p/18145520