# rm_simple_move

一个用于 ROS2 的导航控制桥接节点，作为决策层和控制层的连接。支持两种控制模式：PID 直接控制和 Navigation 导航。

---

## 功能特性

### 双模式控制
- **PID 模式 (mode=0)**: 直接根据当前位置和目标位置进行 PID 控制
- **NAV 模式 (mode=1)**: 将目标转发给 Navigation 系统进行路径规划和导航

### 统一接口
- 单一输入话题：`/nav_goal` (包含模式和目标点)
- 标准输出：`/cmd_vel` (标准 ROS2 速度控制)
- 状态反馈：当前位置和导航状态

---

## 安装依赖

### 1. 克隆仓库
```bash
git clone https://github.com/goldenfishs/rm_simple_move.git
```

### 2. 安装 rm_msgs
```bash
git clone https://github.com/goldenfishs/rm_msgs.git
```

---

## 话题接口

### 输入话题

#### `/nav_goal` (rm_msgs/NavGoal)
导航目标 (合并了模式和目标点)
- `uint8 mode`: 控制模式 (0=PID, 1=NAV)
- `float32 target_x`: 目标 x 坐标
- `float32 target_y`: 目标 y 坐标
- `float32 target_angle`: 目标角度 (-π ~ π)
- `float32 max_speed`: 最大速度 (仅 PID 模式使用)
- `float32 tolerance`: 到达容差 (仅 PID 模式使用)

### 输出话题

#### `/cmd_vel_move` (geometry_msgs/Twist)
统一的速度控制指令输出（底盘订阅此话题）
- `linear.x`: x 方向线速度
- `linear.y`: y 方向线速度
- `angular.z`: z 轴角速度

#### `/nav_status` (rm_msgs/NavStatus)
导航状态
- `uint8 status`: 状态码 (0=IDLE, 1=NAVIGATING, 2=REACHED, 3=FAILED)
- `float32 distance`: 距离目标点的距离
- `string message`: 状态描述

#### `/current_pose` (geometry_msgs/PoseStamped)
当前位置 (map 坐标系)

#### `/goal_pose` (geometry_msgs/PoseStamped)
导航目标 (NAV 模式下发布，供导航系统使用)

---

## 参数配置

| 参数名 | 类型 | 默认值 | 说明 |
|--------|------|--------|------|
| `pid_linear_kp` | double | 0.8 | 线性 PID 比例系数 |
| `pid_linear_ki` | double | 0.0 | 线性 PID 积分系数 |
| `pid_linear_kd` | double | 0.1 | 线性 PID 微分系数 |
| `pid_angular_kp` | double | 1.5 | 角度 PID 比例系数 |
| `pid_angular_ki` | double | 0.0 | 角度 PID 积分系数 |
| `pid_angular_kd` | double | 0.2 | 角度 PID 微分系数 |

---

## 使用方法

### 1. 编译
```bash
cd ~/MOVE_AI
colcon build --packages-select rm_msgs rm_simpal_move
```

### 2. 启动节点
```bash
source install/setup.bash
ros2 launch rm_simpal_move simple_move.launch.py
```

### 3. 测试

#### 使用测试脚本
```bash
cd src/rm_nav/rm_simple_move/test
python3 move_test_new.py
```

测试命令：
```bash
# PID 模式导航
>>> pid 1.0 2.0 0.0

# PID 模式导航 (带速度和容差)
>>> pid 1.0 2.0 0.0 1.5 0.2

# NAV 模式导航
>>> nav 1.0 2.0 0.0

# 查看状态
>>> status

# 退出
>>> quit
```

#### 使用命令行

**PID 模式：**
```bash
ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
  mode: 0,
  target_x: 1.0,
  target_y: 2.0,
  target_angle: 0.0,
  max_speed: 2.0,
  tolerance: 0.1
}" --once
```

**NAV 模式：**
```bash
ros2 topic pub /nav_goal rm_msgs/msg/NavGoal "{
  mode: 1,
  target_x: 1.0,
  target_y: 2.0,
  target_angle: 0.0,
  max_speed: 0.0,
  tolerance: 0.0
}" --once
```

**查看输出：**
```bash
# 查看速度控制指令（底盘订阅此话题）
ros2 topic echo /cmd_vel_move

# 查看导航状态
ros2 topic echo /nav_status

# 查看当前位置
ros2 topic echo /current_pose
```

---

## 工作原理

### PID 模式
1. 接收目标点 `/nav_goal` (mode=0)
2. 通过 TF 获取当前位置 (map -> base_link)
3. 计算目标点在 base_link 坐标系下的相对位置
4. 使用 PID 控制器计算速度指令
5. 发布到 `/cmd_vel_move`

### NAV 模式
1. 接收目标点 `/nav_goal` (mode=1)
2. 转换为 `geometry_msgs/PoseStamped` 格式
3. 发布到 `/goal_pose` 给导航系统
4. 导航系统输出 `/cmd_vel`
5. `rm_simple_move` 订阅 `/cmd_vel` 并转发到 `/cmd_vel_move`

**注意**: 导航系统内部流程为：
```
/goal_pose → Nav2 → /cmd_vel_nav → velocity_smoother → /cmd_vel
```

### 状态发布
- 实时发布当前位置到 `/current_pose`
- 实时发布导航状态到 `/nav_status`
- 包含距离、状态码和描述信息

---

## 架构说明

```
决策层 (Decision Layer)
    ↓ /nav_goal (mode + target)

rm_simple_move (Bridge)
    ├─ PID 模式: 直接计算 → /cmd_vel_move
    └─ NAV 模式: /goal_pose → Navigation Stack
                                    ↓
                              /cmd_vel_nav → velocity_smoother → /cmd_vel
                                                                      ↓
                                                    rm_simple_move 转发 → /cmd_vel_move

底盘驱动 (Chassis Driver)
    ← /cmd_vel_move (统一的速度指令)
```

**话题流向**:
- **PID 模式**: `rm_simple_move` 直接计算并发布到 `/cmd_vel_move`
- **NAV 模式**: `rm_simple_move` 发布 `/goal_pose` → Nav2 处理后输出 `/cmd_vel` → `rm_simple_move` 转发到 `/cmd_vel_move`
- **底盘驱动**: 只订阅 `/cmd_vel_move`，避免冲突

---

## Python 示例

```python
import rclpy
from rclpy.node import Node
from rm_msgs.msg import NavGoal, NavStatus

class MyNode(Node):
    def __init__(self):
        super().__init__('my_node')

        # 发布目标
        self.goal_pub = self.create_publisher(NavGoal, '/nav_goal', 10)

        # 订阅状态
        self.status_sub = self.create_subscription(
            NavStatus, '/nav_status', self.status_callback, 10)

    def send_goal_pid(self, x, y, angle):
        """PID 模式"""
        msg = NavGoal()
        msg.mode = 0  # PID
        msg.target_x = x
        msg.target_y = y
        msg.target_angle = angle
        msg.max_speed = 2.0
        msg.tolerance = 0.1
        self.goal_pub.publish(msg)

    def send_goal_nav(self, x, y, angle):
        """NAV 模式"""
        msg = NavGoal()
        msg.mode = 1  # NAV
        msg.target_x = x
        msg.target_y = y
        msg.target_angle = angle
        self.goal_pub.publish(msg)

    def status_callback(self, msg):
        if msg.status == 2:  # REACHED
            self.get_logger().info('目标已到达!')
```

完整示例见 `test/decision_example.py`

---

## 注意事项

1. **坐标系要求**: 需要正确配置 `map -> base_link` 的 TF 变换
2. **PID 参数调优**: 根据实际机器人调整 PID 参数
3. **模式切换**: 每次发送目标时可以切换模式
4. **导航系统**: NAV 模式需要配合 Nav2 或其他导航系统使用
5. **标准接口**: 输出使用标准 `/cmd_vel`，兼容所有 ROS2 底盘驱动

---

## License

MIT License