6、使用有限状态机实现机器人避障行为

使用有限状态机实现机器人避障行为

1. 感知与执行模型

1.1 几何约定

在解决机器人避障问题时，需要先明确使用的几何约定：
- 单位制 ：采用国际单位制（SI），线性速度单位为 m/s，旋转速度单位为 rad/s，线性加速度单位为 m/s² 等。
- 坐标轴方向 ：在 ROS2 中采用右手坐标系，x 轴向前，y 轴向左，z 轴向上。若以胸部为参考原点，x 坐标为负表示在身后，z 坐标为正表示在上方。
- 角度定义 ：角度定义为绕轴的旋转，绕 x 轴旋转有时称为横滚（roll），绕 y 轴旋转称为俯仰（pitch），绕 z 轴旋转称为偏航（yaw）。角度向左旋转为正，角度 0 表示向前，π 表示向后，π/2 表示向左。

1.2 激光传感器信息

在该问题中，使用激光传感器的信息，其话题为 /scan_raw ，消息类型为 sensor_msgs/msg/LaserScan 。可以通过以下命令查看消息格式：

$ ros2 interface show /sensor_msgs/msg/LaserScan

消息格式如下：

# Single scan from a planar laser range-finder
#
# If you have anothe