|
|
# 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可视化
|
|
|
- 支持参数化配置
|
|
|
|
