室外低速自動導航車的設計(4)——APP與機器人內的ROS地圖的同步和匹配

    在很多商用機器人中都涉及到機器人與外部設備(比如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的消息

   機器人反饋機器人實時位置:  

    {
        “navigation_state”:
        {
            “x”:1.23   //注意:這裏指的是機器人相對地圖原點的實際位置,帶小數點的。
            “y:2.34
        }
    }
這樣就獲取到了機器人的位置。

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數據格式將點擊數據下發至機器人。

 

下一小節我們將介紹機器人圖片與原點座標系的變換方法。

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章