ROS小车识别aruco码到达道路中心

    本次任务说明：场地内有三条交会的黑线和一个固定位置的aruco码，

    任务需求为通过小车上的摄像头识别aruco码来判断自身位置，然后到达黑线的交点。

    视频展示：

    思路分析

    为了找到aruco码的位置，小车开始时会一直自转直到自己编写的节点接收到aruco码的姿态信息，然后通过相机反馈的坐标计算偏转角度和车与aruco码间的距离，最后利用三角函数计算出小车到aruco的直线距离

    实现

   自旋相对简单，在此跳过。

    aruco码的姿态读取：打开例程的同时 rostopic list，可以发现有一个/ar_pose_marker话题，对其进行查询 rostopic echo /ar_pose_marker就可以得到相机反馈的数据

   计算数据：从上图可以看到最后一块的四元数，四元数是描述物体绕轴偏转的四个数据，且它们的平方和为1.其中，x指绕x轴旋转，y指绕y轴旋转，z指绕z轴旋转。

   从俯视图可以看出，aruco码的法线与小车和aruco码中点的连线的夹角等于aruco码绕x轴旋转的角度，根据车身高度和aruco码的高度可以从位置坐标z计算出b的大小，又因为a和α已知，所以根据三角函数公式即可计算出c的大小和b,c的夹角