test/task_2

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. 编译包

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

2. 启动系统

ros2 launch t_shape_robot t_shape_robot.launch.py

3. 自定义参数启动

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	是否循环运动

参数调整建议

小型轨迹：

ros2 launch t_shape_robot t_shape_robot.launch.py \
  t_width:=2.0 t_height:=1.5 linear_speed:=0.2

大型轨迹：

ros2 launch t_shape_robot t_shape_robot.launch.py \
  t_width:=5.0 t_height:=4.0 linear_speed:=0.5

高速运动：

ros2 launch t_shape_robot t_shape_robot.launch.py \
  linear_speed:=0.8 angular_speed:=1.0

单次运动（不循环）：

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: 机器人不动？

检查：

# 查看节点是否运行
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: 如何停止运动？

# 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 - 机器人基座坐标系（随机器人移动）

运动学模型

使用简化的差速驱动运动学：

x' = v * cos(θ) * dt
y' = v * sin(θ) * dt
θ' = ω * dt

其中：

• v - 线速度
• ω - 角速度
• θ - 航向角
• dt - 时间步长

许可证

Apache-2.0

作者

ROS2 Developer

更新日志

v1.0.0 (2025-10-06)

• 初始版本
• 实现基本T字形运动控制
• 添加RViz可视化
• 支持参数化配置