在很多商用機器人中都涉及到機器人與外部設備(比如APP)之間的交互。而在交互中,最爲首要的就是地圖與機器人位置的交互了。在這一個章節中,我們介紹一個機器人將內部的地圖,機器人位置等數據與用戶端進行同步的案例。
1. 實際場景與機器人(ROS)地圖的對應關係:
我們來看一下下面這副圖片,這是一個機器人在實際環境中繪製的二維地圖,在圖片的左下角是地圖原點(map所在的位置),在原點上已經標明瞭這個地圖的三維座標軸,其中紅色的代表x軸方向,綠色代表y軸方向。
在ROS的地圖中,地圖同樣是以像素標記的,每一個像素代表0.05m,也就是一個像素是5釐米。比如下圖上的星星,他的位置在像素層面上的座標爲:x:400像素,y:150像素;那麼這個機器人實際的橫座標就應該是:400*0.05=20米,縱座標是150*0.05=7.5m。再換句話說,如果一個地圖的場合寬都是800像素,那麼它能夠代表的實際長和寬度爲800*0.05=40米。
當然,有時候地圖的原點並不一定在左下角,它的原點可以在地圖的任意一個地方。比如說下面這張圖:
這張圖的地圖原點就跑到了地圖圖片裏,那麼假如機器人在原點左下角的位置上,那麼機器人的X和Y座標就會爲負值。
在ROS中,地圖的類型是nav_msgs/OccupancyGrid,每個像素代表的長度以及地圖的原點都被定義在了消息結構體之中:
std_msgs/Header header
nav_msgs/MapMetaData info
time map_load_time
float32 resolution 分辨率
geometry_msgs/Pose origin原點座標
......
geometry_msgs/Quaternion orientation
......
int8[] data 地圖像素信息
2. 地圖座標系與Python canvas鼠標點擊位置之間的對應關係:
我們使用python編寫客戶端的時候(Java於此類似),地圖是要被繪製到canvas,也就是虛擬畫布上面。虛擬畫布是以其左上角爲座標原點(0,0)(如下圖)。我們舉一個例子,現在有一個800x800的地圖,我們需要使用鼠標或觸摸選擇了APP中地圖的左上角,那麼顯然的,在APP中,我們得到的點擊位置是(0,0)。
但是如果我們把這個數據輸入ROS的地圖中,可以看到,在ROS中,(0,0)點位置對應的是左下角。所以造成的問題就會如下圖:
這個問題解決的方法很簡單,就是在用戶端軟件(如APP)中,將點擊的點做一些處理(座標系變換),再將其數據傳輸到機器人上即可。
3. Python Canvas與機器人內ROS之間的數據傳輸(http)
一般的機器人與客戶端的通訊方式採用http方式傳輸信息,http協議的詳細內容請參見新安淺灘的博文:
https://blog.csdn.net/hu694028833/article/details/80862695
整個機器人與Python之間地圖與座標系的傳遞過程中,Python要向機器人獲取三個信息(已知地圖每個像素爲0.05米):
1)機器人中當前地圖的圖片
2)地圖圖片的右下角在機器人座標系中所處的位置(或者說地圖的原點在地圖圖片中所處的像素位置)
3)機器人相對於地圖座標系原點的位置
由於傳輸數據量很大,所以Python客戶端向機器人請求地圖的頻率不能太高(基本爲數秒一次,或只在加載時請求)。而因爲機器人在時刻移動,機器人相對地圖座標原點的位置我們必須要頻繁的請求。所以在數據通訊上,我們將以上三個信息分爲兩類:
1)當前機器人內部地圖圖片以及地圖原點信息(加載地圖,以及繪製地圖時請求)
2)機器人相對地圖原點的位置(數百毫秒,輪詢請求)
例如,Python在機器人加載地圖後,向機器人發送一個名爲: GET /robot_map HTTP/1.1的消息
機器人向Python反饋jpeg壓縮格式的地圖圖片
Python程序向機器人發送一個名爲:GET /robot_map_origin HTTP/1.1的消息
機器人反饋地圖的原點座標:
{
"x": 1.0
"y":1.0
}
這樣就實現了地圖與地圖座標原點的傳輸。
同時,Python每間隔1000ms向機器人發送:GET /robot_state HTTP/1.1的消息
機器人反饋機器人實時位置:
4. Python上顯示機器人位置以及正確發送點擊座標
現在我們得到了地圖,地圖原點,機器人在實際環境中相對於地圖原點的位置。那麼我們就要在地圖中正確顯示出機器人所在位置,具體的方法如下:
1)採用比例變換,將機器人相對於原點的實際位置座標轉換爲像素座標
2)在已知地圖原點在圖片中像素的前提下,將機器人相對於地圖原點的像素座標,映射到相對地圖圖片的像素中座標。
3)在地圖圖片上,按照機器人的像素座標疊加機器人圖片。
4)顯示
如果我們想向機器人輸出一個鼠標或觸摸點擊的座標,我們要執行以下步驟:
1)獲得鼠標點擊的位置像素座標(x,y)
2)對Python Canvas的座標進行Y軸反轉,獲得臨時像素座標(例如地圖高度爲800,點擊了(100,700)位置,那麼發送的Y座標爲800-700=100)
3)在獲取地圖原點的基礎上,利用座標系變換,將臨時像素座標轉換爲相對於原點的像素座標
4)根據像素-實際尺寸的比例變換,將像素座標變爲實際座標
5)通過http的POST結合json數據格式將點擊數據下發至機器人。
下一小節我們將介紹機器人圖片與原點座標系的變換方法。