ROS实验报告三

实验三：利用action实现小海龟的运动

任务：利用action机制实现小海龟的360°转圈运动

一、实验原理(介绍算法或实验思路)

1、实验思路介绍

本实验的主要目的是利用ros2中的action机制实现小海龟的360°转圈运动，具体实现思路：

（1）动作客户端：

• 客户端节点首先初始化ROS2环境，并创建一个动作客户端对象。
• 客户端发送一个包含使能标志的动作目标消息，告诉服务器开始执行360°转圈运动。
• 客户端等待服务器的响应，并在接收到响应后继续等待最终结果。
• 客户端订阅小海龟的位置信息，并在回调函数中输出当前海龟的位置。

（2）动作服务器：

• 服务器节点初始化ROS2环境，并创建一个动作服务器对象。
• 服务器接收到客户端的动作目标后，开始控制小海龟进行360°转圈运动。
• 服务器设置小海龟的线速度和角速度，并在一个循环中持续发布速度消息，直到完成360°的圆周运动。
• 运动完成后，服务器停止发布速度消息，并向客户端返回成功结果。

二、实验步骤(附上必要图片)

1、终端启动小海龟

ros2 run turtlesim turtlesim_node  

2、创建功能包

3、创建编写源文件

4、配置编译文件

5、编译

6、运行客户端以及服务器节点

三、实验结果+分析(附上必要图片)

1、实验过程及结果展示

2、实验过程分析

（1）启动turtlesim节点：

• 成功启动turtlesim节点后，屏幕上显示了一个小海龟。

（2）启动动作服务器节点：

• 成功启动动作服务器节点后，终端输出“Moving circle...”信息，表示服务器已准备好接收动作目标。

（3）启动动作客户端节点：

• 成功启动动作客户端节点后，客户端发送动作目标，并等待服务器响应。客户端订阅小海龟的位置信息，并在回调函数中输出当前海龟的位置。

（4）小海龟运动：

• 小海龟开始按照设定的线速度和角速度进行360°转圈运动。运动过程中，客户端不断输出小海龟的位置信息，确认运动状态。

（5）运动完成：

• 小海龟完成360°转圈运动后，服务器停止发布速度消息，并向客户端返回成功结果。客户端输出最终结果和小海龟的最终位置。

3、实验结果分析

（1）运动轨迹：

小海龟按照预期完成了360°的圆周运动，轨迹呈圆形，没有明显的偏移或误差，这表明设置的线速度和角速度是合理的，能够有效控制小海龟的运动。

（2）反馈机制：

客户端订阅小海龟的位置信息，并在回调函数中实时输出当前位置。这种实时反馈机制有助于监控小海龟的运动状态，确保运动过程的稳定性和准确性。

四、关键功能代码

1、action_client.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import rclpy                              # ROS2 Python接口库
from rclpy.node import Node               # ROS2 节点类
from rclpy.action import ActionClient     # ROS2 动作客户端类
from learning_interface.action import MoveCircle  # 自定义的圆周运动接口
from turtlesim.msg import Pose            # 导入海龟位置消息

class MoveCircleActionClient(Node):
    def __init__(self, name):
        super().__init__(name)            # ROS2节点父类初始化
        self._action_client = ActionClient(  # 创建动作客户端（接口类型、动作名）
            self, MoveCircle, 'move_circle')
        self.pose_subscription = self.create_subscription(  # 创建订阅者，订阅海龟位置
            Pose, '/turtle1/pose', self.pose_callback, 10)
        self.current_pose = None          # 初始化当前海龟位置

    def send_goal(self, enable):          # 创建一个发送动作目标的函数
        goal_msg = MoveCircle.Goal()      # 创建一个动作目标的消息
        goal_msg.enable = enable          # 设置动作目标为使能，希望机器人开始运动

        self._action_client.wait_for_server()  # 等待动作的服务器端启动
        self._send_goal_future = self._action_client.send_goal_async(  # 异步方式发送动作的目标
            goal_msg)  # 动作目标

        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('Goal rejected :(')
            return

        self.get_logger().info('Goal accepted :)')  # 动作被顺利执行

        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('Result: {%d}' % result.finish)  # 日志输出执行结果
        if self.current_pose:
            self.get_logger().info('Final Pose: x=%f, y=%f, theta=%f' % (self.current_pose.x, self.current_pose.y, self.current_pose.theta))

    def pose_callback(self, msg):  # 创建处理海龟中间位置消息的回调函数
        self.current_pose = msg    # 更新当前海龟位置
        self.get_logger().info('Pose updated: x=%f, y=%f, theta=%f' % (msg.x, msg.y, msg.theta))

def main(args=None):  # ROS2节点主入口main函数
    rclpy.init(args=args)  # ROS2 Python接口初始化
    node = MoveCircleActionClient("action_move_client")  # 创建ROS2节点对象并进行初始化
    node.send_goal(True)  # 发送动作目标
    rclpy.spin(node)  # 循环等待ROS2退出
    node.destroy_node()  # 销毁节点对象
    rclpy.shutdown()  # 关闭ROS2 Python接口

if __name__ == '__main__':
    main()

2、action_server.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time
import rclpy                              # ROS2 Python接口库
from rclpy.node import Node               # ROS2 节点类
from rclpy.action import ActionServer     # ROS2 动作服务器类
from learning_interface.action import MoveCircle  # 自定义的圆周运动接口
from geometry_msgs.msg import Twist        # 导入速度消息

class MoveCircleActionServer(Node):
    def __init__(self, name):
        super().__init__(name)            # ROS2节点父类初始化
        self._action_server = ActionServer(  # 创建动作服务器（接口类型、动作名、回调函数）
            self,
            MoveCircle,
            'move_circle',
            self.execute_callback)
        self.publisher_ = self.create_publisher(Twist, '/turtle1/cmd_vel', 10)  # 创建速度发布者

    def execute_callback(self, goal_handle):  # 执行收到动作目标之后的处理函数
        self.get_logger().info('Moving circle...')
        twist = Twist()                       # 创建速度消息

        # 设置线速度和角速度
        twist.linear.x = 1.0
        twist.angular.z = 1.0

        start_time = self.get_clock().now().nanoseconds / 1e9  # 获取当前时间（秒）
        duration = 6.28  # 360度大约需要6.28秒（2π秒）

        while (self.get_clock().now().nanoseconds / 1e9 - start_time) < duration:
            self.publisher_.publish(twist)  # 发布速度消息
            time.sleep(0.1)  # 每0.1秒发布一次速度消息

        twist.linear.x = 0.0
        twist.angular.z = 0.0
        self.publisher_.publish(twist)  # 停止海龟

        goal_handle.succeed()  # 动作执行成功
        result = MoveCircle.Result()  # 创建结果消息
        result.finish = True
        return result  # 反馈最终动作执行的结果

def main(args=None):  # ROS2节点主入口main函数
    rclpy.init(args=args)  # ROS2 Python接口初始化
    node = MoveCircleActionServer("action_move_server")  # 创建ROS2节点对象并进行初始化
    rclpy.spin(node)  # 循环等待ROS2退出
    node.destroy_node()  # 销毁节点对象
    rclpy.shutdown()  # 关闭ROS2 Python接口

if __name__ == '__main__':
    main()