laneser

LEGO NXT 玩膩了?

沒關係, 模擬器就是有這種好處, LEGO 只有碰撞感應, 或是超音波偵測而已, 要玩就玩大一點的, 來玩 Pioneer P3DX 模擬, 這個上面裝有 Sick LRF (Laser Range Finder)LMS200 ... 這東東據說也要 6K USD ... 怎麼辦, 都玩這麼貴的虛擬寶物, 胃口都被養大了.

那我們該如何入手來玩這東西, 當然, 先用一個 RDS 已經弄好的 manifest 是最快的.

我們還是可以使用 VPL , 拖一個 GenericDifferentialDrive 來入手的 (微軟的打算就是, 通通用我的統一規格, 將來微軟還是搞軟體, 硬體就交給其他人去搞割喉戰吧...哈哈)

這次 manifest 我們選 SimpleVisionSim.Manifest.Xml , 這樣我們就可以跑看看了.

這...這傢伙要四萬美金啊...
不過, 你應該同時注意到, 還多了兩個對話框, 一個是 Vision, 一個是 Dashboard , 都還不錯玩.

稍微解釋 Dashboard 的玩法, 它長的就像這樣:

你應該先選擇 Remote Node 的 Connect (預設是 localhost), 點選 Connect 以後, 你會看到兩個 Device (P3DXMotorBase, P3DXLasterRangeFinder), 此時的 Laser Range Finder 可以點選 connect 看結果, 而左邊的 Differential Drive 則需要你雙擊 (Double Click) P3DXMotorBase 以後, 就會呈現 Motor: On 的狀況, 這時你再去點選 Direct Input Device 當中的 Drive, 就可以用那個虛擬的 JoyStick (一個圈圈畫十字的那個) 來操控機器車子.

我們就從如何讀取 SickLRF 來入手吧, 這個東西比超音波好玩多了.

你可以在網路上找到關於 SickLRF 的相關資料, 像是

http://www.hizook.com/blog/2008/12/15/sick-laser-rangefinder-lidar-disassembled

http://www.pages.drexel.edu/~kws23/tutorials/sick/sick.html

不過玩過 RDS 以後, 相信你會覺得微軟搞出來的東西好像還真的比較好的樣子...

如果你對 LMS-200 有點概念, 它其實就像雷達一樣, 用掃描的方式來得知周遭物體的距離. 所以我們可以得到一系列的距離數字(從不同角度打出去的雷射所計算出的距離, 以 LMS-200 為例, 你可以取得 180 個距離(有可能會根據解析度而有不同的數量, 數值是 0 ~ 8000 左右, 單位是 mm (0.001 m)), 分別表示從左邊水平線到右邊水平線的距離, 所以你知道第 90 個數字(或者中間那個數字) 表示最前頭的距離.).

現在我們開始寫一個 DSS 來取得 LRF 的資料, 然後顯示出來  (就像是 Dashboard 的 Laser Range Finder 那一塊)

首先用 VS2008 新開一個 DSS Service 2.0 的專案, 取名為 LRFDrive.

這次我們的 partners 要選兩個, 一個是 Sick Laser Range Finder, 另一個則是 Generic Differential Drive (因為我們之後要來操控機器車)
但是我們這次這兩個的 CreatePolicy 都要選 UseExisting (採用現有的), 這樣可以避免跟 VPL 裡面已經載入的 Manifest 相衝, 也可以讓它最後(等到所有 Partner Service 完成後)才載入. 
但也因為採用 UseExisting, 所以這個 Service 沒辦法獨立從 VS2008 執行. (也許你更改了它的 Manifest 就可以在 VS 2008 內執行, 不過我都在 VPL 裡面玩)

接下來, 我們要參考一個 .NET 的 dll - Microsoft.Ccr.Adapters.WinForms (你可以從 .NET 參考中找到它, 不是 RDS 的 bin 目錄歐), 顧名思義, 這個是 CCR 提供用來跟 WinForms 界接的介面, 目的就是因為 Windows Form UI 只能在 Form Thread 當中操控 UI 元件的關係.

加完參考後, 我們可以加入一個 Windows Form , 就取名叫做 LRFStatus 好了, 放一個 PictureBox 元件在上面, 把 PictureBox 的 name 改為 pic, SizeMode 改為 StretchImage (自動縮放) , 如下:

現在我們可以到 LRFDriveService class 當中加入這樣的宣告

public LRFStatus LRFForm { get; set; }

 

那這個 LRFForm 就應該在 Start() 函式內被建立出來囉, 是的, 但是要透過 Ccr.Adapters.WinForms, 所以你要加 using Microsoft.Ccr.Adapters.WinForms;
然後 Start() 改為如下的 code

/// <summary>
/// Service start
/// </summary>
protected override void Start()
{

    // 
    // Add service specific initialization here
    // 

    base.Start();

    WinFormsServicePort.Post(new RunForm(() =>
        {
            LRFForm = new LRFStatus();
            return LRFForm;
        }));
}

 

這樣子, 我們回到 VPL 的環境下 (如果你不想重新啟動 VPL , 你可以在 VPL 的 View 當中選 Reload Services), 把 LRFDrive 這個 Service 拖到 Diagram 當中,

一旦執行以後, 就會看到我們的 LRFStatus dialog 有跳出來 (最後才跳出來歐, 因為我們是用 UseExisting)

那我們接下來要怎麼跟 SickLRF 溝通? 你應該有注意到 Partner service 的 code :

/// <summary>
/// SickLRFService partner
/// </summary>
[Partner("SickLRFService", Contract = sicklrf.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
sicklrf.SickLRFOperations _sickLRFServicePort = new sicklrf.SickLRFOperations();

 

我們可以利用 _sickLRFServicePort 來跟 SickLRF 溝通, 採用註冊的方式 (Subscribe) , 一旦收到 Replace (狀態被全部改變) 就呼叫我們的處理函式,
所以再把 Start() 改成如下的 code

/// <summary>
/// Service start
/// </summary>
protected override void Start()
{

    // 
    // Add service specific initialization here
    // 

    base.Start();

    WinFormsServicePort.Post(new RunForm(() =>
        {
            LRFForm = new LRFStatus();
            return LRFForm;
        }));


    sicklrf.SickLRFOperations _laserNotify = new sicklrf.SickLRFOperations();
    _sickLRFServicePort.Subscribe(_laserNotify);
    Activate(Arbiter.ReceiveWithIterator<sicklrf.Replace>(true, _laserNotify, LrfReplaceHandler));

}

新增加的三行 code , 第一行就是產生一個 Port (嚴格來說是 PortSet, 可以接受 SickLRF 的所有訊息),
第二行把這個 port 向 SickLRF Service 的 mainport 註冊,
第三行是利用 Activate 這個函式, 啟動一個 Reciver 的 Task, Reciver 會在 _laserNotify 這個 port 收到 sicklrf.Replace 這樣的訊息時, 執行 LrfReplaceHandler, 第一個 true 的參數表示這個 Reciver 的 Task 是一直執行的 (在程式執行期間都是存在的) . 而 LrfReplaceHandler 的 code 如下:

public IEnumerator<ITask> LrfReplaceHandler(sicklrf.Replace replace)
{
    WinFormsServicePort.FormInvoke( () =>
        {
            LRFForm.ReplaceLaserData(replace.Body);
        });

    yield break;
}

 

終於, 就是收到訊息時, 訊息內容的 Body 就是我們要的 LRF 的資訊, 而我們要把它畫成圖片顯示, 所以要在 FromInvoke 當中呼叫 (因為所有 WinForm UI 都要在 UI Thread 當中被執行).

最後, LRFStatus class (那個 Windows Form )當中還要寫一個 ReplaceLaserData 的函式, 這個函式內容是抄自 Dashboard 的 source code, 我認為不難理解, 所以就不囉嗦了, 如下:

public void ReplaceLaserData(sicklrf.State stateType)
{
    Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
    Graphics g = Graphics.FromImage(bmp);
    g.Clear(Color.LightGray);

    int half = bmp.Height / 2;

    for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
    {
        int range = stateType.DistanceMeasurements[x];
        if (range > 0 && range < 8192)
        {
            int h = bmp.Height * 500 / stateType.DistanceMeasurements[x];
            if (h < 0)
            {
                h = 0;
            }
            Color col = LinearColor(Color.DarkBlue, Color.LightGray, 0, 8192, range);
            g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
        }
    }
    pic.Image = bmp;
}

private Color LinearColor(Color nearColor, Color farColor, int nearLimit, int farLimit, int value)
{
    if (value <= nearLimit)
    {
        return nearColor;
    }
    else if (value >= farLimit)
    {
        return farColor;
    }

    int span = farLimit - nearLimit;
    int pos = value - nearLimit;

    int r = (nearColor.R * (span - pos) + farColor.R * pos) / span;
    int g = (nearColor.G * (span - pos) + farColor.G * pos) / span;
    int b = (nearColor.B * (span - pos) + farColor.B * pos) / span;

    return Color.FromArgb(r, g, b);
}

 

於是, 再一次執行 VPL , 你就會發現我們的 LRFStatus 上面的圖形跟 Dashboard 的 Laser Range Finder 一樣囉. (大小不一樣是因為我們用 StretchImage, 我們不需要點選 Connect 才會出現, 因為我們在 Start() 的時候就自己去註冊搞定連線了...)

執行的畫面如下 (可以想像成一開始透過 LRF 看的畫面, 然後透過 dashboard 去玩機器人會更有感覺歐):