# T字形轨迹机器人控制包

ROS2功能包，控制机器人执行T字形运动轨迹。

## 功能特性

- 完整的小车URDF模型
- T字形轨迹运动控制
- 实时路径可视化
- TF坐标变换发布
- 支持循环/单次运动模式
- RViz实时3D可视化

## 工程结构

```
t_shape_robot/
├── config/
│   └── params.yaml
├── launch/
│   └── t_shape_robot.launch.py
├── rviz/
│   └── config.rviz
├── t_shape_robot/
│   ├── __init__.py
│   └── t_shape_controller.py
├── urdf/
│   └── robot.urdf
├── package.xml
├── setup.py
├── setup.cfg
└── README.md
```

## 系统要求

- ROS2 (Humble/Foxy/Galactic)
- Python 3.8+
- RViz2

## 快速开始

### 1. 编译包

```bash
cd ~/ros2_ws/src
cd ~/ros2_ws
colcon build --packages-select t_shape_robot
source install/setup.bash
```

### 2. 启动系统

```bash
ros2 launch t_shape_robot t_shape_robot.launch.py
```

### 3. 自定义参数启动

```bash
ros2 launch t_shape_robot t_shape_robot.launch.py \
  t_width:=4.0 \
  t_height:=3.0 \
  linear_speed:=0.5 \
  angular_speed:=0.8
```

## 参数说明

| 参数名 | 类型 | 默认值 | 说明 |
|--------|------|--------|------|
| `t_width` | float | 3.0 | T字横杠宽度（米） |
| `t_height` | float | 2.0 | T字竖杠高度（米） |
| `linear_speed` | float | 0.3 | 直线运动速度（米/秒） |
| `angular_speed` | float | 0.5 | 旋转速度（弧度/秒） |
| `loop_enabled` | bool | true | 是否循环运动 |

### 参数调整建议

小型轨迹：
```bash
ros2 launch t_shape_robot t_shape_robot.launch.py \
  t_width:=2.0 t_height:=1.5 linear_speed:=0.2
```

**大型轨迹：**
```bash
ros2 launch t_shape_robot t_shape_robot.launch.py \
  t_width:=5.0 t_height:=4.0 linear_speed:=0.5
```

**高速运动：**
```bash
ros2 launch t_shape_robot t_shape_robot.launch.py \
  linear_speed:=0.8 angular_speed:=1.0
```

**单次运动（不循环）：**
```bash
ros2 launch t_shape_robot t_shape_robot.launch.py \
  loop_enabled:=false
```

## 运动轨迹说明

T字形运动分为10个阶段（完整遍历所有边）：

1. **前进** - 沿竖杠从起点到T字中心（t_height距离）
2. **左转90°** - 转向横杠左侧方向
3. **左移** - 沿横杠从中心到左端（t_width/2距离）
4. **右转180°** - 在左端掉头
5. **右移** - 沿横杠从左端经过中心到右端（t_width完整宽度）
6. **右转180°** - 在右端掉头（返回中心）✅
7. **左移** - 沿横杠从右端返回中心（t_width/2距离）✅
8. **左转90°** - 在中心转向竖杠方向
9. **后退** - 沿竖杠从中心返回起点（t_height距离）
10. **左转180°** - 回到初始朝向

完成一个循环后，如果`loop_enabled=true`，会自动开始下一轮。机器人会完整遍历T字形的所有边并精确回到起点。

## 话题说明

### 发布的话题

| 话题名 | 消息类型 | 说明 |
|--------|----------|------|
| `/cmd_vel` | `geometry_msgs/Twist` | 速度控制指令 |
| `/robot_path` | `nav_msgs/Path` | 机器人运动路径 |
| `/t_shape_marker` | `visualization_msgs/Marker` | T字形参考轨迹 |
| `/robot_description` | `std_msgs/String` | 机器人URDF描述 |
| `/tf` | `tf2_msgs/TFMessage` | 坐标变换 |

## RViz可视化

启动后RViz会显示：

1. **网格** - 参考坐标网格
2. **TF坐标系** - odom → base_link及所有关节
3. **机器人模型** - 3D小车模型（蓝色底盘、黑色轮子、红色传感器平台）
4. **运动路径** - 绿色轨迹线（实时轨迹）
5. **参考轨迹** - 红色半透明T字形标记（计划轨迹）

### RViz操作技巧

- **旋转视角**：鼠标左键拖动
- **平移视角**：Shift + 鼠标左键拖动
- **缩放**：鼠标滚轮
- **重置视角**：在Views面板中调整Current View参数

## 代码说明

### 核心节点：t_shape_controller

**主要功能：**
- 计算T字形运动轨迹
- 发布速度控制指令
- 更新机器人位姿（里程计）
- 发布TF变换
- 记录和发布运动路径

**关键方法：**
- `control_loop()` - 主控制循环（50Hz），生成速度指令
- `update_position()` - 更新机器人位姿（运动学模型）
- `publish_status()` - 发布TF和路径（10Hz）
- `publish_t_shape_marker()` - 发布参考轨迹标记

## 常见问题

### Q1: 机器人不动？

**检查：**
```bash
# 查看节点是否运行
ros2 node list

# 查看速度指令是否发布
ros2 topic echo /cmd_vel

# 查看节点日志
ros2 node info /t_shape_controller
```

### Q2: RViz中看不到机器人？

**解决：**
- 确保Fixed Frame设置为`odom`
- 检查Robot Model显示项是否启用
- 查看TF是否正常发布：`ros2 run tf2_ros tf2_echo odom base_link`

### Q3: 路径不显示？

**解决：**
- 在RViz左侧Displays面板中启用"Robot Path"
- 检查话题：`ros2 topic echo /robot_path`

### Q4: 如何修改小车外观？

编辑`urdf/robot.urdf`文件，修改：
- `<box size="...">` - 底盘尺寸
- `<cylinder radius="..." length="...">` - 轮子大小
- `<color rgba="...">` - 颜色（RGBA值）

### Q5: 如何停止运动？

```bash
# Ctrl+C 终止launch文件
# 或单独停止控制器节点
ros2 lifecycle set /t_shape_controller shutdown
```

## 扩展开发

### 修改运动轨迹

编辑`t_shape_robot/t_shape_controller.py`中的`control_loop()`方法，修改各个阶段的运动逻辑。

### 添加新的运动模式

可以创建新的控制器节点，参考现有代码结构：
1. 继承`Node`类
2. 发布`/cmd_vel`话题
3. 更新位姿并发布TF
4. 记录路径

### 集成到仿真环境

本包可以集成到Gazebo仿真：
1. 在URDF中添加Gazebo插件
2. 添加差速驱动控制器
3. 配置物理参数

## 技术细节

### 坐标系

- `odom` - 里程计坐标系（固定参考系）
- `base_link` - 机器人基座坐标系（随机器人移动）

### 运动学模型

使用简化的差速驱动运动学：
```python
x' = v * cos(θ) * dt
y' = v * sin(θ) * dt
θ' = ω * dt
```

其中：
- `v` - 线速度
- `ω` - 角速度
- `θ` - 航向角
- `dt` - 时间步长

## 许可证

Apache-2.0

## 作者

ROS2 Developer

## 更新日志

### v1.0.0 (2025-10-06)
- 初始版本
- 实现基本T字形运动控制
- 添加RViz可视化
- 支持参数化配置