ROS实验报告三

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

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

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

1、实验思路介绍

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

(1)动作客户端:
  • 客户端节点首先初始化ROS2环境,并创建一个动作客户端对象。
  • 客户端发送一个包含使能标志的动作目标消息,告诉服务器开始执行360°转圈运动。
  • 客户端等待服务器的响应,并在接收到响应后继续等待最终结果。
  • 客户端订阅小海龟的位置信息,并在回调函数中输出当前海龟的位置。
(2)动作服务器:
  • 服务器节点初始化ROS2环境,并创建一个动作服务器对象。
  • 服务器接收到客户端的动作目标后,开始控制小海龟进行360°转圈运动。
  • 服务器设置小海龟的线速度和角速度,并在一个循环中持续发布速度消息,直到完成360°的圆周运动。
  • 运动完成后,服务器停止发布速度消息,并向客户端返回成功结果。

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

1、终端启动小海龟

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
#!/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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
#!/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()

ROS实验报告三
http://jrhu0048.github.io/2024/11/09/ros/ros-shi-yan-bao-gao-san/
作者
JR.HU
发布于
2024年11月9日
更新于
2024年11月9日
许可协议