From 86f0c06c31398bfb6e88b2b5b3bf67fb7b9f46cd Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Fri, 13 Mar 2026 00:27:17 +0800 Subject: [PATCH] add test --- src/rm_decision/CMakeLists.txt | 8 +- src/rm_decision/test/README.md | 79 ++++++++++++ src/rm_decision/test/simple_nav_test.py | 156 ++++++++++++++++++++++++ 3 files changed, 242 insertions(+), 1 deletion(-) create mode 100644 src/rm_decision/test/README.md create mode 100755 src/rm_decision/test/simple_nav_test.py diff --git a/src/rm_decision/CMakeLists.txt b/src/rm_decision/CMakeLists.txt index 0853a62..b841bb4 100644 --- a/src/rm_decision/CMakeLists.txt +++ b/src/rm_decision/CMakeLists.txt @@ -44,10 +44,16 @@ install(DIRECTORY include/ DESTINATION include ) -install(DIRECTORY launch +install(DIRECTORY launch test DESTINATION share/${PROJECT_NAME} ) +# 安装 Python 测试脚本 +install(PROGRAMS + test/simple_nav_test.py + DESTINATION lib/${PROJECT_NAME} +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() diff --git a/src/rm_decision/test/README.md b/src/rm_decision/test/README.md new file mode 100644 index 0000000..d06c936 --- /dev/null +++ b/src/rm_decision/test/README.md @@ -0,0 +1,79 @@ +# 简单导航测试脚本 + +## 快速使用 + +```bash +# 直接运行 +python3 src/rm_decision/test/simple_nav_test.py + +# 或者通过 ros2 run +ros2 run rm_decision simple_nav_test.py +``` + +## 修改目标点 + +直接编辑 `simple_nav_test.py` 文件中的 `waypoints` 列表： + +```python +# 格式: [x, y, angle, max_speed, tolerance, name] +self.waypoints = [ + [0.0, 0.0, 0.0, 1.0, 0.15, "起点"], + [1.0, 0.0, 0.0, 1.5, 0.15, "右"], + [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"], + [0.0, 1.0, math.pi, 1.5, 0.15, "左上"], + [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"], +] +``` + +### 参数说明 + +- `x, y`: 目标坐标（米） +- `angle`: 目标角度（弧度） + - 0° = 0 + - 90° = math.pi/2 = 1.57 + - 180° = math.pi = 3.14 + - -90° = -math.pi/2 = -1.57 +- `max_speed`: 最大速度（米/秒） +- `tolerance`: 到达容差（米） +- `name`: 目标点名称（用于日志显示） + +## 示例输出 + +``` +[simple_nav_test]: ======================================== +[simple_nav_test]: 简单导航测试脚本启动 +[simple_nav_test]: 目标点数量: 5 +[simple_nav_test]: ======================================== +[simple_nav_test]: [0] 起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15 +[simple_nav_test]: [1] 右: (1.00, 0.00, 0°) speed=1.5 tol=0.15 +[simple_nav_test]: [2] 右上: (1.00, 1.00, 90°) speed=1.5 tol=0.15 +[simple_nav_test]: [3] 左上: (0.00, 1.00, 180°) speed=1.5 tol=0.15 +[simple_nav_test]: [4] 回到起点: (0.00, 0.00, 0°) speed=1.0 tol=0.15 +[simple_nav_test]: ======================================== +[simple_nav_test]: 📍 发送目标点 [1/5] 起点: (0.00, 0.00, 0°) speed=1.0 +[simple_nav_test]: 🚗 导航中 [1/5] 起点: 距离=0.50m 状态=1 (5s) +[simple_nav_test]: ✓ 到达目标点 [1/5] 起点 (用时 8.2秒) +[simple_nav_test]: 📍 发送目标点 [2/5] 右: (1.00, 0.00, 0°) speed=1.5 +... +[simple_nav_test]: ✅ 所有目标点完成！ +``` + +## 优点 + +- ✅ 无需编译，直接运行 +- ✅ 修改目标点只需编辑 Python 代码 +- ✅ 代码简单易懂 +- ✅ 实时显示导航进度 +- ✅ 自动处理超时和失败 + +## 与 C++ 版本对比 + +| 特性 | Python 脚本 | C++ test_nav_node | +|------|------------|-------------------| +| 修改目标点 | 编辑 Python 代码 | 编辑 YAML 配置 | +| 运行方式 | 直接运行 | 需要编译 | +| 灵活性 | 高（可随时修改） | 中（需重新编译） | +| 性能 | 足够 | 更高 | +| 适用场景 | 快速测试 | 生产环境 | + +推荐使用 Python 脚本进行快速测试！ diff --git a/src/rm_decision/test/simple_nav_test.py b/src/rm_decision/test/simple_nav_test.py new file mode 100755 index 0000000..a5ce3a5 --- /dev/null +++ b/src/rm_decision/test/simple_nav_test.py @@ -0,0 +1,156 @@ +#!/usr/bin/env python3 +""" +简单的导航测试脚本 +直接在代码中定义目标点，通过 /nav_goal 话题发送给 simple_move +""" + +import rclpy +from rclpy.node import Node +from rm_msgs.msg import NavGoal, NavStatus +import time +import math + + +class SimpleNavTest(Node): + def __init__(self): + super().__init__('simple_nav_test') + + # 发布者和订阅者 + self.nav_goal_pub = self.create_publisher(NavGoal, '/nav_goal', 10) + self.nav_status_sub = self.create_subscription( + NavStatus, '/nav_status', self.nav_status_callback, 10) + + # 状态 + self.current_status = None + self.current_distance = 0.0 + + # 定义目标点列表 [x, y, angle, max_speed, tolerance, name] + self.waypoints = [ + [0.0, 0.0, 0.0, 1.0, 0.15, "起点"], + [1.0, 0.0, 0.0, 1.5, 0.15, "右"], + [1.0, 1.0, math.pi/2, 1.5, 0.15, "右上"], + [0.0, 1.0, math.pi, 1.5, 0.15, "左上"], + [0.0, 0.0, 0.0, 1.0, 0.15, "回到起点"], + ] + + self.current_index = 0 + self.goal_sent = False + self.goal_timeout = 30.0 # 超时时间（秒） + self.goal_start_time = None + + self.get_logger().info('========================================') + self.get_logger().info('简单导航测试脚本启动') + self.get_logger().info(f'目标点数量: {len(self.waypoints)}') + self.get_logger().info('========================================') + + # 显示所有目标点 + for i, wp in enumerate(self.waypoints): + self.get_logger().info( + f' [{i}] {wp[5]}: ({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) ' + f'speed={wp[3]:.1f} tol={wp[4]:.2f}') + + self.get_logger().info('========================================') + + # 等待2秒后开始 + time.sleep(2.0) + self.send_next_goal() + + # 创建定时器检查状态 + self.timer = self.create_timer(0.5, self.check_status) + + def nav_status_callback(self, msg): + """接收导航状态""" + self.current_status = msg.status + self.current_distance = msg.distance + + def send_next_goal(self): + """发送下一个目标点""" + if self.current_index >= len(self.waypoints): + self.get_logger().info('✅ 所有目标点完成！') + rclpy.shutdown() + return + + wp = self.waypoints[self.current_index] + + # 创建导航目标消息 + goal_msg = NavGoal() + goal_msg.control_mode = 0 # 0: PID模式 + goal_msg.x = float(wp[0]) + goal_msg.y = float(wp[1]) + goal_msg.angle = float(wp[2]) + goal_msg.max_speed = float(wp[3]) + goal_msg.tolerance = float(wp[4]) + + # 发布目标 + self.nav_goal_pub.publish(goal_msg) + self.goal_sent = True + self.goal_start_time = time.time() + + self.get_logger().info( + f'📍 发送目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: ' + f'({wp[0]:.2f}, {wp[1]:.2f}, {wp[2]*180/math.pi:.0f}°) speed={wp[3]:.1f}') + + def check_status(self): + """检查导航状态""" + if not self.goal_sent: + return + + if self.current_status is None: + return + + wp = self.waypoints[self.current_index] + elapsed = time.time() - self.goal_start_time + + # 检查是否到达 (status == 2) + if self.current_status == 2: + self.get_logger().info( + f'✓ 到达目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} ' + f'(用时 {elapsed:.1f}秒)') + self.current_index += 1 + self.goal_sent = False + time.sleep(1.0) # 等待1秒 + self.send_next_goal() + return + + # 检查是否失败 (status == 3) + if self.current_status == 3: + self.get_logger().warn( + f'✗ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} 导航失败') + self.current_index += 1 + self.goal_sent = False + time.sleep(1.0) + self.send_next_goal() + return + + # 检查超时 + if elapsed > self.goal_timeout: + self.get_logger().warn( + f'⏱ 目标点 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]} ' + f'超时 ({elapsed:.1f}秒)，跳过') + self.current_index += 1 + self.goal_sent = False + time.sleep(1.0) + self.send_next_goal() + return + + # 每5秒显示一次进度 + if int(elapsed) % 5 == 0 and int(elapsed * 2) % 2 == 0: + self.get_logger().info( + f'🚗 导航中 [{self.current_index + 1}/{len(self.waypoints)}] {wp[5]}: ' + f'距离={self.current_distance:.2f}m 状态={self.current_status} ({elapsed:.0f}s)') + + +def main(args=None): + rclpy.init(args=args) + + try: + node = SimpleNavTest() + rclpy.spin(node) + except KeyboardInterrupt: + print('\n用户中断') + finally: + rclpy.shutdown() + + +if __name__ == '__main__': + main()