260415
好吧，最近在忙毕业这边的事情，所以可能有时候不会更新。
继我们上次写的 follow_controller节点。
如果当前模式是 FOLLOW，则会计算当前机器人和旗帜的距离和目标方向，然后生产一个 Twist 发布到 /cmd_vel_raw。
我们是将机器人和旗帜的一系列数据都直接存储在内存中的，各自变量对应：
- self.robot_x, self.robot_y 机器人当前平面位置
- self.robot_yaw 机器人当前朝向
- self.flag_x, self.flag_y 旗帜当前平面位置
由于我们目前并不考虑 z 轴，因此我们只定义了机器人和旗帜的 x&y 坐标。
该坐标从 odom（里程计） 话题中获取。
因此就有：
- self.robot_x = msg.pose.pose.position.x
- self.robot_y = msg.pose.pose.position.y
如此来获取到机器人当前的位置。
然后就是朝向了，这个 yaw 也是来自于 odom 中，但是它并不是直接给一个角度，而是从姿态的四元数中来进行计算的。
我们姿态的四元数格式是这样的：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotOne$ ros2 interface show nav_msgs/msg/Odometry
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id
# Includes the frame id of the pose parent.
std_msgs/Header header
builtin_interfaces/Time stamp
int32 sec
uint32 nanosec
string frame_id
# Frame id the pose points to. The twist is in this coordinate frame.
string child_frame_id
# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose
Pose pose
Point position
float64 x
float64 y
float64 z
Quaternion orientation
float64 x 0
float64 y 0
float64 z 0
float64 w 1
float64[36] covariance
# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist
Twist twist
Vector3 linear
float64 x
float64 y
float64 z
Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
我们解析一下这个大名鼎鼎的 Odometry。
这里最外层是四个字段，分别是 header,child_frame_id ,pose 和 twist。
首先是 header。
其下有两个字段，分别是 stamp 和 frame_id。
前者是时间戳，精确到纳秒，允许进行极其精细的时间对齐。
后者是父坐标系，通常都是 odom，这个父坐标系在我们的 Gazebo 中获取倒是很简单，但是如果要到了现实中维护和获取就比较麻烦了，我们这里先不进行展开。
然后是child_frame_id 了，这是指相对什么的坐标系来进行计算，我们这里是底盘，在我们的 SDF 文件中也有写到的。
...
<child_frame_id>chassis</child_frame_id>
...
再往下，第三个，也就是 pose，这里面有两个字段来表示姿态，分别是 pose 和 orientation。
前者表示坐标，后者则表示方向。
后者的默认值为 0 0 0 1，即表示没有任何转动。
这里你可能会注意到，我们除了欧拉角的 roll, pitch, yaw 分别对应的 x, y, z 以外，多出来了一个w。
这个 w 就是构成四元数的关键。
四元数认为，无论你的机器人在空中翻转成了多么奇怪的姿态，我都可以通过仅仅一次旋转来完成。
这个“仅仅一次旋转”的角度则触及到了欧拉旋转定理。
欧拉旋转定理认为，在 3D 空间中，必然存在一根神奇的、斜着穿过魔方中心的隐形棍子。你只要找到这根棍子，把它固定死，然后捏住物体绕着这根棍子只扭动一次（比如扭 91 度），物体就能瞬间到达那个复杂的姿态。
那为什么不直接用欧拉角来进行计算姿态呢？
虽然欧拉角很符合我们人类的认知习惯，先绕 x 轴旋转 a 度，绕 y 轴旋转 b 度，最后再绕 z 轴旋转 c 度，就可以达到目标姿态。
但是欧拉角对于计算机来说则是一个很大的问题，比如当 Pitch 旋转到 90 度（比如目标头部绝对垂直朝上）时，Roll 和 Yaw 的旋转轴会重合，导致丢失一个自由度，此时数学计算会由于除零而直接崩溃。
所以为了解决计算机的计算问题，我们在底层通信则直接强制使用四元数来进行计算，而不使用欧拉角。
四元数的底层逻辑也叫轴-角表示法，这也是我们上面拿一根隐形的棍子作比喻的原因，这个隐形的棍子就是轴。
而四元数中的四个数字，则就是由这根棍子的方向+旋转的角度计算而来的。
具体的计算过程我就不进行展开了，这里不是数学讲堂。
总的来说，我们可以通过 Roll, Pitch, Yaw 来计算出这个旋转轴的方向和需要旋转的目标角度。
我们继续往下，接下来是 covariance 这个字段。
这个字段翻译过来就是协方差矩阵。
这个字段是一个 6*6 的矩阵，即对应 x, y, z, roll, pitch, yaw 用来表示这条消息中 pose 的置信度。
比如，机器人的轮子刚才可能打滑了，物理引擎或底层传感器觉得当前算出来的 x 坐标不准。它就会在矩阵里把 x 对应的方差值调大，告诉上层算法这个值很可能计算的有问题，最好是相信激光雷达的数据。
最下面就是我们一开始在海龟模拟的时候就见到的 Twist 了，用来表示线速度和角速度，同理，也有一个 covariance，功能和原理同上。
这就是我们对 Odometry 这个消息的拆解了。
我们继续。
朝向的计算重点在于这几行代码：
siny_cosp = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
cosy_cosp = 1.0 - 2.0 * (orientation.y * orientation.y + orientation.z * orientation.z)
self.robot_yaw = math.atan2(siny_cosp, cosy_cosp)
这是把四元数转为平面朝向角
然后就是计算机器人到旗帜的相对位移：
dx = self.flag_x - self.robot_x
dy = self.flag_y - self.robot_y
distance = math.hypot(dx, dy)
如果已经进入停止距离，则发 0 速：
if distance <= stop_distance:
self.cmd_vel_raw_publisher.publish(Twist())
return
这就是“到旗帜附近就停下”的实现。
然后是计算目标方向和当前朝向的差值：
target_yaw = math.atan2(dy, dx)
heading_error = self._normalize_angle(target_yaw - self.robot_yaw)
最后就是计算并发布这个速度了，不过线速度和角速度我们都有限制最大值的：
linear_speed = min(
cast(float, self.get_parameter('max_linear_speed').value),
cast(float, self.get_parameter('linear_gain').value) * max(0.0, distance - stop_distance),
)
angular_speed = max(
-cast(float, self.get_parameter('max_angular_speed').value),
min(
cast(float, self.get_parameter('max_angular_speed').value),
cast(float, self.get_parameter('angular_gain').value) * heading_error,
),
)
command = Twist()
command.angular.z = angular_speed
if abs(heading_error) < cast(float, self.get_parameter('heading_tolerance').value):
command.linear.x = linear_speed
self.cmd_vel_raw_publisher.publish(command)
即距离/角度差值越大，则线速度/角速度越快，但有个限制。
就这样，我们实现了旗帜跟踪器。
以上。
接下来我们进入第二阶段——给机器人接入激光雷达。
首先是接入激光雷达到我们的模型的 sdf 文件中，我们加入一个新的 link：
<link name="lidar_link">
<pose>0.20 0 0.165 0 0 0</pose>
<inertial>
<mass>0.20</mass>
<inertia>
<ixx>0.0005</ixx>
<ixy>0.0</ixy>
<ixz>0.0</ixz>
<iyy>0.0005</iyy>
<iyz>0.0</iyz>
<izz>0.0005</izz>
</inertia>
</inertial>
<collision name="lidar_collision">
<geometry>
<cylinder>
<radius>0.04</radius>
<length>0.05</length>
</cylinder>
</geometry>
</collision>
<visual name="lidar_visual">
<geometry>
<cylinder>
<radius>0.04</radius>
<length>0.05</length>
</cylinder>
</geometry>
<material>
<ambient>0.10 0.10 0.12 1</ambient>
<diffuse>0.16 0.16 0.18 1</diffuse>
<specular>0.05 0.05 0.05 1</specular>
</material>
</visual>
<sensor name="top_lidar" type="gpu_lidar">
<topic>/scan</topic>
<update_rate>20</update_rate>
<always_on>true</always_on>
<visualize>true</visualize>
<gz_frame_id>lidar_link</gz_frame_id>
<lidar>
<scan>
<horizontal>
<samples>720</samples>
<resolution>1</resolution>
<min_angle>-3.14159</min_angle>
<max_angle>3.14159</max_angle>
</horizontal>
</scan>
<range>
<min>0.12</min>
<max>12.0</max>
<resolution>0.01</resolution>
</range>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.005</stddev>
</noise>
</lidar>
</sensor>
</link>
然后加入一个新的固定 joint 来进行连接：
<joint name="lidar_joint" type="fixed">
<parent>chassis</parent>
<child>lidar_link</child>
</joint>
顶上的小圆柱就是我们的这个传感器：
我们来进行逐个解释。
其他的我们不用说，对于传感器中心的 z 坐标，我们则是计算底盘顶面高度+传感器的高度/2来进行计算得到的。
我们之前的代码中，底盘的中心点的 z 坐标为0.08，然后它的高度为0.12，即我们顶面的坐标为0.08 + 0.12/2=0.14。
然后我们传感器圆柱的高为0.05，因此最终的高度就为0.14 + 0.05/2 = 0.165得到的。
这里我们可以看到一个之前没有见过的标签——sensor。
目前我们传感器所在的结构是这样的：
model
└── link: lidar_link
└── sensor: top_lidar
这里的lidar_link 只是雷达的安装载体
所以我们这里就创建了一个名为top_lidar，类型为gpu_lidar的传感器，这个gpu_lidar是 Gazebo 内置的 GPU 激光雷达。
雷达的原理是是从中心放出成百上千条射线，去和 3D 世界中的墙壁、树木、障碍物等复杂的网格模型进行求算交点，就算我们配置每一圈发射 720 条射线，以 20hz 的频率来进行旋转，那也相当于每秒要计算 14.4k 次高精度的空间碰撞计算。
如果将这个任务丢给我们的 CPU 来进行计算，我们世界的 real_time_factor 会低到几乎无法运行。
而我因为带有 4060，且 GPU 天生就是为了进行大规模并行射线追踪和图形渲染用的，因此使用 gpu_lidar 会大幅降低我们 CPU 的负担。
继续往下，我们看实际的标签字段。
首先是其中的 topic 字段，这个表示发布激光雷达的消息到哪个话题，我们设定为之前就想好的 /scan 话题。
update_rate 也就是电机的转速，这里我们填为 20，相当于是每秒 20 转，这个性能还是不错的。
always_on 则是为了让传感器一直工作，visualize 是为了让我们可以看到传感器在 gz 中扫描时候的射线，但是我们现在障碍物很少，所以看不到，我们后续接入避障时会引入很多障碍物，到时候就可以看见了。
gz_frame_id则是用来确定扫描数据的相对坐标系的，这样才能确定扫描数据的实际位姿。
然后往下就是lidar了，这个标签用来确定“怎么扫”。
首先是 horizontal 这个标签，表示水平方向上的扫描配置，
这里samples表示采样数，表示一圈采样的点数量，我们这里一圈采样 720 各点。
min_angle和max_angle则代表采样的角度，我们这里是从 -180 度到 180 度。
然后是 range ，是表示测距能力，毕竟测距能力并不是无限大的。
这里表示最小为 0.12m，最大为 12m。
resolution 在这里则为分辨率，在horizontal下表示每度的密度，我们这里设置为 1；在 range 下则是距离的分辨率，我们这里设置为 0.01。
在horizontal下我们设置为 0.5 的话，则是 360/(720 * 0.5) = 1，即 1 度采样一个点，实际采样到的点数量为 360 个。
在range下的话则是改变汇报结果的分辨率，比如我们的一个物体距离激光雷达 3.14159 米，那我们这里配置为 0.01 则只能检测到距离 3.14m，后面的精度就会被丢弃或者认为不可靠，而如果改为 0.001 的话，则可能会检测为 3.142 米，精度是要更高的。
最后就是噪声 noise 了，我们这里设置为高斯噪声，均值为 0，标准差为 0.005m,这使得我们的设备会更像真实的设备。
不过还没完，我们需要引入传感器插件到我们的世界 src/kibot_one_sim/worlds/kibot_one.world.sdf 中：
<plugin
filename="gz-sim-sensors-system"
name="gz::sim::systems::Sensors">
<render_engine>ogre2</render_engine>
</plugin>
这样我们的传感器才能正常工作。
在 src/kibot_one_sim/package.xml 中引入传感器消息的依赖：
<exec_depend>sensor_msgs</exec_depend>
然后在 src/kibot_one_sim/config/ros_gz_bridge.yaml 建立一个新的桥接：
- ros_topic_name: "/scan"
gz_topic_name: "/scan"
ros_type_name: "sensor_msgs/msg/LaserScan"
gz_type_name: "gz.msgs.LaserScan"
direction: GZ_TO_ROS
最后拉起来 echo 看看：
(.venv) jese--ki@KiBall:~/Projects/dev/KiBots/KiBotOne$ ros2 topic echo /scan --once
header:
stamp:
sec: 2049
nanosec: 852000000
frame_id: lidar_link
angle_min: -3.141590118408203
angle_max: 3.141590118408203
angle_increment: 0.008738775737583637
time_increment: 0.0
scan_time: 0.0
range_min: 0.11999999731779099
range_max: 12.0
ranges:
- .inf
- .inf
... # 手动省略
- .inf
- '...'
intensities:
- 0.0
- 0.0
... # 手动省略
- 0.0
- 0.0
- '...'
---
可以看到我们的激光雷达已经是正常工作了。
就这样，我们完成了第二步，引入激光雷达。
以上。
