? ? 本次任務(wù)說明：場地內(nèi)有三條交會(huì)的黑線和一個(gè)固定位置的aruco碼，

? ? 任務(wù)需求為通過小車上的攝像頭識(shí)別aruco碼來判斷自身位置，然后到達(dá)黑線的交點(diǎn)。

? ? 視頻展示：

? ? 思路分析

? ? 為了找到aruco碼的位置，小車開始時(shí)會(huì)一直自轉(zhuǎn)直到自己編寫的節(jié)點(diǎn)接收到aruco碼的姿態(tài)信息，然后通過相機(jī)反饋的坐標(biāo)計(jì)算偏轉(zhuǎn)角度和車與aruco碼間的距離，最后利用三角函數(shù)計(jì)算出小車到aruco的直線距離

? ? 實(shí)現(xiàn)

? ?自旋相對(duì)簡單，在此跳過。

? ??aruco碼的姿態(tài)讀?。捍蜷_例程的同時(shí) rostopic list，可以發(fā)現(xiàn)有一個(gè)/ar_pose_marker話題，對(duì)其進(jìn)行查詢 rostopic echo /ar_pose_marker就可以得到相機(jī)反饋的數(shù)據(jù)

? ?計(jì)算數(shù)據(jù)：從上圖可以看到最后一塊的四元數(shù)，四元數(shù)是描述物體繞軸偏轉(zhuǎn)的四個(gè)數(shù)據(jù)，且它們的平方和為1.其中，x指繞x軸旋轉(zhuǎn)，y指繞y軸旋轉(zhuǎn)，z指繞z軸旋轉(zhuǎn)。

? ?從俯視圖可以看出，aruco碼的法線與小車和aruco碼中點(diǎn)的連線的夾角等于aruco碼繞x軸旋轉(zhuǎn)的角度，根據(jù)車身高度和aruco碼的高度可以從位置坐標(biāo)z計(jì)算出b的大小，又因?yàn)閍和α已知，所以根據(jù)三角函數(shù)公式即可計(jì)算出c的大小和b,c的夾角