[Robotics Studio] P3DX[III] - 繼續畫地圖 -- Day15 | laneser - 點部落

2009-01-02

[Robotics Studio] P3DX[III] - 繼續畫地圖 -- Day15

5098
0
2009-02-17

[Robotics Studio] P3DX[III] - 繼續畫地圖 -- Day15

Day14 的畫地圖程式只能畫當下的 LRF 狀態.

其實我們只剩下最後一步了, 就是把自身在地圖上的定位找到, 然後藉由移動自己, 重複畫地圖, 最後你就擁有一張大地圖...

這概念其實很簡單, 但是實做起來就很頭痛.

本來我是沒在怕的, 因為看樣子人家已經做出來了, 就是那個 ExplorerSim, 結果實際看完 code , 才發現是一點小作弊, 就是把模擬環境 (VSE) 當中的機器人位置直接傳回作為定位...
(難怪 GenericvDifferentialDrive 提供的大部分屬性值通通為 0 , 我還以為是 bug... 但好歹你給我個 CurrentPower, or CurrentSpeed 也好, 都沒有, 這個不知道是不是 bug)

現實的機器人世界, 除了戶外定位的 GPS 以外, 好像沒這麼好的感應器啊 XD
大部分的解決辦法是透過外部系統的參考點, 算出自己的位置, 比方, 藉由攝影機看到幾個地標 (Landmarks) 然後根據那些地標算出自己跟地標之間的相互距離.

恩, 在不是很精準的情況下, 我們可以透過 GenericDifferentialDrive 所提供的 DriveDistance, RotateDegrees 來做其實也可以達到效果.
(但是因為 GenericDifferentialDrive 的屬性值大都為 0, 所以如果你透過 Dashboard 去操控機器人, 我們的地圖就錯亂啦)

所以, 基於這個想法, 可以在 LRFStatus Form 上面加上三個 Button, 一個是 Scan Map (from LRF status) , 一個是往前進一個距離 (DriveDistance), 然後 Scan Map, 另外一個是旋轉(RotateDegrees) 然後 Scan Map, 做完這樣的練習, 我們應該就對 DSS 的訊息傳輸接收機制有點心得.

為了接收 Button 的指令, 當然我們得要先定義 DataContract (就是訊息的內容 (body)), 如下:


[DataContract]
public class ScanMapData
{
    [DataMember]
    public double ForwardScan;

    [DataMember]
    public double RotateScan;
}

然後, 為了這個需求, 我們還要在收到指令後, 算出下一次 ScanMap 在地圖當中的位置, 所以在狀態當中新增 NextPosition, 還有, 我們要開始移動了, 地圖要大一點才行, 所以狀態的定義如下 :


/// <summary>
/// LRFDrive state
/// </summary>
[DataContract]
public class LRFDriveState
{
    [DataMember]
    public DrivePos CurrntPosition;

    [DataMember]
    public DrivePos NextPosition;

    [DataMember]
    public drive.DriveDifferentialTwoWheelState DriveState;

    [DataMember]
    public sicklrf.State LrfState;

    [DataMember]
    public MapBlockInfo Map;

    public LRFDriveState()
    {
        CurrntPosition = new DrivePos();
        Map = LRFMapDrawer.CreateMapBlock(-15, -15, 30, 30, 0.05);
    }
}

接著, 我們還要接收 GenericDifferentialDrive 的兩個訊息 (DriveDistance, RotateDegrees) , 這個工作我們之前在 VPL 做過, 現在在 DSS 當作再做一次, 架構上當然是收到通知後, 再轉發訊息給我們自己, 這樣比較簡單. 所以我們又多了兩個接收訊息的 DSSP , 現在 LRFDriveOperations 的定義如下:


/// <summary>
/// LRFDrive main operations port
/// </summary>
[ServicePort]
public class LRFDriveOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, Subscribe, UpdateDrive, UpdateLRF, ScanMapCommand, UpdateDriveDistance, UpdateDriveRotate>
{
}
    
public class UpdateDrive : Update<drive.DriveDifferentialTwoWheelState, PortSet<DefaultUpdateResponseType, Fault>> { }

public class UpdateLRF : Update<sicklrf.State, PortSet<DefaultUpdateResponseType, Fault>> { }    

public class ScanMapCommand : Update<ScanMapData, PortSet<DefaultUpdateResponseType, Fault>> { }

public class UpdateDriveDistance : Update<drive.DriveDistanceRequest, PortSet<DefaultUpdateResponseType, Fault>> { }

public class UpdateDriveRotate : Update<drive.RotateDegreesRequest, PortSet<DefaultUpdateResponseType, Fault>> { }

剩下來的就是把通知轉為訊息發給我們自己, 當然是透過 Activate 加上 Reciver 來做, 所以我們可以在 Start() 加上 :


Activate(Arbiter.Receive<drive.DriveDistance>(true, _driveNotify, update => _mainPort.Post(new UpdateDriveDistance() { Body = update.Body })));
Activate(Arbiter.Receive<drive.RotateDegrees>(true, _driveNotify, update => _mainPort.Post(new UpdateDriveRotate() { Body = update.Body })));

再來, 就是把 ScanMapCommand, UpdateDriveDistance, UpdateDriveRotate 的 Handler 寫好, 如下:


[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> ScanMapCommandHandler(ScanMapCommand smd)
{
    if ((smd.Body.ForwardScan == 0) && (smd.Body.RotateScan == 0))
    {
        LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
        UpdateMap();
        UpdatePosInfo();
    }
    else if (smd.Body.ForwardScan > 0)
    {
        _state.NextPosition = new DrivePos()
        {
            Direction = _state.CurrntPosition.Direction,
            X = _state.CurrntPosition.X + smd.Body.ForwardScan * Math.Cos(_state.CurrntPosition.Direction),
            Y = _state.CurrntPosition.Y + smd.Body.ForwardScan * Math.Sin(_state.CurrntPosition.Direction)
        };
        _driveDifferentialTwoWheelPort.Post(new drive.DriveDistance(
            new drive.DriveDistanceRequest(smd.Body.ForwardScan, 0.3)));
    }
    else if (smd.Body.RotateScan > 0)
    {
        double currentdegree = (_state.CurrntPosition.Direction / Math.PI) * 180.0;
        double newdegree = currentdegree - smd.Body.RotateScan;
        if (newdegree > 360)
            newdegree -= 360;
        if (newdegree < -360)
            newdegree += 360;

        _state.NextPosition = new DrivePos()
        {
            Direction = newdegree * Math.PI / 180.0,
            X = _state.CurrntPosition.X,
            Y = _state.CurrntPosition.Y
        };

        _driveDifferentialTwoWheelPort.Post(new drive.RotateDegrees(
            new drive.RotateDegreesRequest(smd.Body.RotateScan, 0.2)));
    }

    smd.ResponsePort.Post(DefaultUpdateResponseType.Instance);

    yield break;
}
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> UpdateDriveDistanceHandler(UpdateDriveDistance update)
{
    if ((_state.NextPosition != null) && (update.Body.DriveDistanceStage == Microsoft.Robotics.Services.Drive.Proxy.DriveStage.Completed))
    {
        _state.CurrntPosition = new DrivePos()
        {
            Direction = _state.NextPosition.Direction,
            X = _state.NextPosition.X,
            Y = _state.NextPosition.Y
        };
        UpdatePosInfo();
        Activate(Arbiter.Receive(false, TimeoutPort(1000), time => _mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));
    }
    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
    yield break;
}


[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> UpdateDriveRotateHandler(UpdateDriveRotate update)
{
    if ((_state.NextPosition != null) && (update.Body.RotateDegreesStage == Microsoft.Robotics.Services.Drive.Proxy.DriveStage.Completed))
    {
        _state.CurrntPosition = new DrivePos()
        {
            Direction = _state.NextPosition.Direction,
            X = _state.NextPosition.X,
            Y = _state.NextPosition.Y
        };
        UpdatePosInfo();
        Activate(Arbiter.Receive(false, TimeoutPort(1000), time => _mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));
    }
    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
    yield break;
}

從 code 你可以知道, 我們收到 ForwardScan == 0, RotateScan == 0 時就馬上 ScanMap, 否則就丟給 GenericDifferentialDrive, 之後收到通知以後, 再等一秒 (因為你知道的, DriveDistance 都是暴力停止, 在這個情況下, LRF 會傳回地面資料給我們) 然後再要求 ScanMap. 當然, 之前的 UpdateLRFHandler 要改為:


[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> UpdateLRFHandler(UpdateLRF update)
{
    _state.LrfState = update.Body;
    UpdateLRFImage();
    //LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
    //UpdateMap();
    //UpdatePosInfo();
    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
    yield break;
}

最後, 要怎麼從 LRFStatus Form 去呼叫我們的 LRFDrive DSS Service ? 當然是把自己這個 MainPort 丟給它最簡單, 所以在 LRFStatus 當中加上這個屬性:


public LRFDriveOperations LRFPort;

然後在 start() 生出 Form 的時候把值設定好, 像是這樣:


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

最後在 Form 弄三個 Button, 按下去的時候分別做以下的動作:


private void button1_Click(object sender, EventArgs e)
{
    LRFPort.Post(new ScanMapCommand()
    {
        Body = new ScanMapData() { RotateScan = 0, ForwardScan = 1 }
    });
}

private void button2_Click(object sender, EventArgs e)
{
    LRFPort.Post(new ScanMapCommand()
    {
        Body = new ScanMapData() { RotateScan = 0, ForwardScan = 0 }
    });
}

private void button3_Click(object sender, EventArgs e)
{
    LRFPort.Post(new ScanMapCommand()
    {
        Body = new ScanMapData() { RotateScan = 90, ForwardScan = 0 }
    });
}

好! 去 VPL 玩玩看:

假設定位精準 (現實上 DriveDistance, RotateDegrees 有誤差, LRF 也會有誤差), 我們應該可以畫出整個地圖...

Robotics

回首頁