继之前分享的一篇《Robotics Studio学习教程:第十三天——让我们一起来玩P3DX》, 我们将继续分享下一篇《Robotics Studio学习教程:第十四天——利用P3DX画地图》,让我继续开始我们学习Visual Programming Language,以及使用Robotics Studio学习开发机器人应用的道路吧。
[Robotics Studio] P3DX [II] 根据我在艾泽拉斯大陆的经验, 迷路要看地图 – Day14
是的, 做了 LRF 的立体示意图 (其实是假的, 因为 LRF 只能侦测平面而已) , 我们的机器人应该还是会迷路吧...
所以我们有必要写程序来描绘地图...这样才有办法做出能够自走迷宫的机器人啊! (这是我每次看人家比赛的梦想)
我想起了在艾泽拉斯开地图的经验... 所以我们就来根据这个经验, 好好探索未知的领域吧. 首先要修改 Day 13 的程序, 先将 _lasterNotify 移出 Start() 让它成为 Property , 好让其他 Member Function 也可以用, 我也一并加上 _dirveNotify , 把它们跟 partner service 放在一起如下:
- /// <summary>
- /// 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 如下:
- [DataContract]
- 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;
- 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 宣告如下:
- /// <summary>
- /// 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 如下:
- public void UpdateMap(MapBlockInfo map)
- {
- 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 如下:
- /// <summary>
- /// 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 的函式如下:
- [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;
- }
- [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 ?)
让我们继续一下章教程:
《Robotics Studio学习教程:第十五天——继续利用P3DX画地图》