add test

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()

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 脚本进行快速测试！

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()