[Robotics Studio] P3DX [II] 根據我在艾澤拉斯大陸的經驗, 迷路要看地圖 -- Day14

2009-01-01

5717
0
2009-04-08

是的, 做了 LRF 的立體示意圖 (其實是假的, 因為 LRF 只能偵測平面而已) , 我們的機器人應該還是會迷路吧...

所以我們有必要寫程式來描繪地圖...這樣才有辦法做出能夠自走迷宮的機器人啊! (這是我每次看人家比賽的夢想)

我想起了在艾澤拉斯開地圖的經驗... 所以我們就來根據這個經驗, 好好探索未知的領域吧.

首先要修改 Day 13 的程式, 先將 _lasterNotify 移出 Start() 讓它成為 Property , 好讓其他 Member Function 也可以用,
我也一併加上 _dirveNotify , 把它們跟 partner service 放在一起如下:


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

/// <summary>
/// DriveDifferentialTwoWheel partner
/// </summary>
[Partner("DriveDifferentialTwoWheel", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
drive.DriveOperations _driveDifferentialTwoWheelPort = new drive.DriveOperations();
drive.DriveOperations _driveNotify = new drive.DriveOperations();

有沒有發現都一模一樣? 只是 partner service 多了屬性的宣告而已.

接下來, 我要修改一下之前的架構, 之前是收到 _laserNotify 的變更(replace) 之後, 直接呼叫我們的 reciver 當中的 handler, 這樣我們沒有把 LRF 的狀態存下來以供使用, 但為了要畫地圖, 有必要存下來, 所以要修改成為收到 _laserNotify 的變更 (replace)後, 發送一個變更 (update) 通知給我們自己.
為了要畫地圖, 還要新增很多東西, 像是自身的位置 (因為是平面地圖, 所以只有 x,y 以及面向的方向) , 還有地圖的資料, 這些通通是我們的 _state, 所以變更 LRFDriveTypes.cs 如下:


public class DrivePos
{
    /// <summary>
    /// radians  ( * PI/180 = degree)
    /// </summary>
    [DataMember]
    public double Direction;

    [DataMember]
    public double X;

    [DataMember]
    public double Y;

    [DataMember]
    public DateTime TimeStamp;

    public DrivePos()
    {
        TimeStamp = DateTime.Now;
        Direction = (double)-90.0 * Math.PI / 180.0;
    }
}

[DataContract]
public class MapBlockInfo
{
    /// <summary>
    /// Unit mill
    /// </summary>
    [DataMember]
    public double Top;

    [DataMember]
    public double Left;

    [DataMember]
    public double Width;

    [DataMember]
    public double Height;

    /// <summary>
    /// 1 cell (mapdata) size in meter
    /// </summary>
    [DataMember]
    public double Resolution;

    [DataMember]
    public byte[,] MapData;

    [DataMember]
    public int MapDataWidthMax;

    [DataMember]
    public int MapDataHeightMax;
}

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

    [DataMember]
    public drive.DriveDifferentialTwoWheelState DriveState;

    [DataMember]
    public sicklrf.State LrfState;

    [DataMember]
    public MapBlockInfo Map;

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

由上面你可以發現, 我定義了一堆資料型態, 然後在初始化 _state 的時候, 把自己定在 0,0 , 面向 -90 度 (正北, 因為把地圖北方設為負的位置, 採螢幕座標) 的位置, 然後開一張 20x20 , 中心點也是 0, 解析度是 0.05 m 的地圖. 當然, 你會發現 LRFMapDrawer 是啥? 我另外寫了專門畫地圖的程式, 雖然有參考別人的 (ExplorerSim), 但是我有做過修改, 如下:


using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using sicklrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;

namespace LRFDrive
{
    public class LRFMapDrawer
    {
        /// <summary>
        /// 0 ~ 127 means occupied, 129 ~ 255 means Vacant , 128 means unknown
        /// </summary>
        public static byte UnknownMapValue = 128;

        public static MapBlockInfo CreateMapBlock(double top, double left, double width, double height, double resolution)
        {
            int resw = (int)Math.Ceiling(width / resolution);
            int resh = (int)Math.Ceiling(height / resolution);
            byte[,] mapdata = new byte[resw, resh];

            for (int x = 0; x < resw; x++)
                for (int y = 0; y < resh; y++)
                    mapdata[x, y] = UnknownMapValue;

            return new MapBlockInfo()
            {
                Top = top,
                Left = left,
                Width = width,
                Height = height,
                Resolution = resolution,
                MapData = mapdata,
                MapDataHeightMax = resh,
                MapDataWidthMax = resw,
            };
        }

        public static void DrawMap(sicklrf.State lrfdata, MapBlockInfo map, DrivePos pos)
        {
            double currentangle = pos.Direction * 180.0 / Math.PI;
            double angle = currentangle + ((double)lrfdata.AngularRange)/2;
            foreach (int len in lrfdata.DistanceMeasurements)
            {
                double radians = angle * Math.PI / 180;
                double length = len * 0.001;
                bool IsHitted = (len < 8000);

                double dx = length * Math.Cos(radians);
                double dy = length * Math.Sin(radians);
                double EndPointX = pos.X + dx;
                double EndPointY = pos.Y + dy;
                double distance = Math.Sqrt(dx * dx + dy * dy);

                int step = (int)Math.Ceiling(distance/map.Resolution);
                for (int i = 0; i < step; i++)
                {
                    double dstep = (double)i / (double)step;
                    SetMapValue(map, pos.X + dx * dstep, pos.Y + dy * dstep, 255);
                }

                if (IsHitted)
                {
                    SetMapValue(map, EndPointX, EndPointY, 0);
                }

                
                angle -= lrfdata.AngularResolution;
            }
        }
       
        private static void SetMapValue(MapBlockInfo map, double x, double y, byte value)
        {
            int px = (int)Math.Floor((x - map.Left) / map.Resolution);
            int py = (int)Math.Floor((y - map.Top) / map.Resolution);
            if ((px >= 0) && (px < map.MapDataWidthMax) && (py >= 0) && (py < map.MapDataHeightMax))
                map.MapData[px, py] = value;
        }


        public static Bitmap CreateBitmap(MapBlockInfo map)
        {
            Bitmap bmp = new Bitmap(map.MapDataWidthMax, map.MapDataHeightMax, PixelFormat.Format24bppRgb);
            BitmapData bmpdata = bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height),
                ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);

            byte[] data = new byte[bmpdata.Stride * bmp.Height];

            int k = 0;
            int linestep = 0;
            for (int y = 0; y < bmp.Height ; y++)
            {
                k = linestep;
                for (int x = 0; x < bmp.Width; x++)
                {
                    data[k++] = map.MapData[x, y];
                    data[k++] = map.MapData[x, y];
                    data[k++] = map.MapData[x, y];
                }
                linestep += bmpdata.Stride;
            }

            Marshal.Copy(data, 0, bmpdata.Scan0, data.Length);

            bmp.UnlockBits(bmpdata);
            return bmp;
        }
    }
}

我稍微解釋一下地圖是怎麼畫的, 首先根據解析度, 把地圖切為小方塊, 每一個方塊用 byte 來代表 (byte=128 代表未知, 0~127表示有東西佔住, 129~255 表示空的).
當收到 LRF 傳來的資料, 就根據自身的位置以及面向的角度, 畫出前方的狀態.
最後還有一個把地圖畫成 Bitmap 的函式.

一開始我提到要改架構, 所以現在我們新增了幾個 Update DSSP 宣告如下:


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

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

好了, 我們可以把 LRFStatus Form 改為加上一個地圖的 Picture (位置隨你放, 參數自己定喜歡的, 我把名稱定為 picmap),
我還多放了一個 ToolStripStatus, 裡面放了 Label 用來表示狀態, 然後 LRFStatus Form 新增的 code 如下:


{
    picmap.Image = LRFMapDrawer.CreateBitmap(map);
}

public void UpdatePosInfo(DrivePos pos)
{
    tsLabelPosition.Text = string.Format("X:{0}, Y:{1}, Dir:{2} [{3}]",
        pos.X, pos.Y, pos.Direction, pos.TimeStamp.ToString("hh:mm:ss.fff"));
}

最後, 我們要來更改 Start() , code 如下:


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

    // 
    // Add service specific initialization here
    // 

    base.Start();

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

    // Subscribe Notification Ports
    _sickLRFServicePort.Subscribe(_laserNotify);
    _driveDifferentialTwoWheelPort.Subscribe(_driveNotify);

    // Activate reciver.
    Activate(Arbiter.Receive<sicklrf.Replace>(true, _laserNotify, replace => _mainPort.Post(new UpdateLRF() {Body = replace.Body })));
    Activate(Arbiter.Receive<drive.Update>(true, _driveNotify, update => _mainPort.Post(new UpdateDrive() { Body = update.Body })));
}

你可以發現到, 之前是收到通知, 就做事, 現在因為我們要做的事情是要改變自身狀態 (state), 所以最好是透過 Post 一個變更訊息給自己,
這樣才會在 CCR 的要求下維持資料的正確性.
Handler 的函式如下:


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;
}

[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> UpdateDriveHandler(UpdateDrive update)
{
    _state.DriveState = update.Body;
    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
    yield break;
}

private void UpdateMap()
{
    WinFormsServicePort.FormInvoke(() =>
    {
        LRFForm.UpdateMap(_state.Map);
    });
}

private void UpdatePosInfo()
{
    WinFormsServicePort.FormInvoke(() =>
    {
        LRFForm.UpdatePosInfo(_state.CurrntPosition);
    });
}

private void UpdateLRFImage()
{
    WinFormsServicePort.FormInvoke(() =>
    {
        LRFForm.ReplaceLaserData(_state.LrfState);
    });

}

比較多的東西是在收到 LRF 變更資料的時候做了很多事情.
按照 code , 就是先把 LRF 的資料存到自己的狀態當中, 然後畫 LRF , 畫地圖, 把地圖畫出來.
恩, 今天都是一堆 code...
放張最後執行的地圖結果上來:

你覺得上下兩張圖, 有沒有差別呢?
(如果你試著去操控車子...你就知道, 定位不正確會畫出啥鳥圖, 天啊, 室內要何時才有精準的 GPS ?)

回首頁