iMotoLab

Robotics Studio学习教程

继之前分享的一篇《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 放在一起如下:

1.   /// <summary>
2.   /// SickLRFService partner
3.   /// </summary>
4. [Partner("SickLRFService", Contract = sicklrf.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
5. sicklrf.SickLRFOperations _sickLRFServicePort = new sicklrf.SickLRFOperations();
6. sicklrf.SickLRFOperations _laserNotify = new sicklrf.SickLRFOperations();
7. 
   
8.  /// <summary>
9.  /// DriveDifferentialTwoWheel partner
10.  /// </summary>
11. [Partner("DriveDifferentialTwoWheel", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
12. drive.DriveOperations _driveDifferentialTwoWheelPort = new drive.DriveOperations();
13. drive.DriveOperations _driveNotify = new drive.DriveOperations();

 

有没有发现都一模一样? 只是 partner service 多了属性的宣告而已.

接下来, 我要修改一下之前的架构, 之前是收到 _laserNotify 的变更(replace) 之后, 直接呼叫我们的 reciver 当中的 handler, 这样我们没有把 LRF 的状态存下来以供使用, 但为了要画地图, 有必要存下来, 所以要修改成为 收到 _laserNotify 的变更 (replace)后, 发送一个变更 (update) 通知给我们自己.

为了要画地图, 还要新增很多东西, 像是自身的位置 (因为是平面地图, 所以只有 x,y 以及面向的方向) , 还有地图的数据, 这些通通是我们的 _state, 所以变更 LRFDriveTypes.cs 如下:

1.   [DataContract]
2.   public class DrivePos
3.   {
4.       /// <summary>
5.       /// radians  ( * PI/180 = degree)
6.       /// </summary>
7.       [DataMember]
8.       public double Direction;
9. 
   
10.       [DataMember]
11.       public double X;
12. 
    
13.       [DataMember]
14.       public double Y;
15. 
    
16.       [DataMember]
17.       public DateTime TimeStamp;
18. 
    
19.       public DrivePos()
20.       {
21.           TimeStamp = DateTime.Now;
22.           Direction = (double)-90.0 * Math.PI / 180.0;
23.       }
24.   }
25. 
    
26.   [DataContract]
27.   public class MapBlockInfo
28.   {
29.       /// <summary>
30.       /// Unit mill
31.       /// </summary>
32.       [DataMember]
33.       public double Top;
34. 
    
35.       [DataMember]
36.       public double Left;
37. 
    
38.       [DataMember]
39.       public double Width;
40. 
    
41.       [DataMember]
42.       public double Height;
43. 
    
44.       /// <summary>
45.       /// 1 cell (mapdata) size in meter
46.       /// </summary>
47.       [DataMember]
48.       public double Resolution;
49. 
    
50.       [DataMember]
51.       public byte[,] MapData;
52. 
    
53.       [DataMember]
54.       public int MapDataWidthMax;
55. 
    
56.       [DataMember]
57.       public int MapDataHeightMax;
58.   }
59. 
    
60.   /// <summary>
61.   /// LRFDrive state
62.   /// </summary>
63.   [DataContract]
64.   public class LRFDriveState
65.   {
66.       [DataMember]
67.       public DrivePos CurrntPosition;
68. 
    
69.       [DataMember]
70.       public drive.DriveDifferentialTwoWheelState DriveState;
71. 
    
72.       [DataMember]
73.       public sicklrf.State LrfState;
74. 
    
75.       [DataMember]
76.       public MapBlockInfo Map;
77. 
    
78.       public LRFDriveState()
79.       {
80.           CurrntPosition = new DrivePos();
81.           Map = LRFMapDrawer.CreateMapBlock(-10, -10, 20, 20, 0.05);
82.       }
83.   }

 

由上面你可以发现, 我定义了一堆数据型态, 然后在初始化 _state 的时候, 把自己定在 0,0 , 面向 -90 度 (正北, 因为把地图北方设为负的位置, 采屏幕坐标) 的位置, 然后开一张 20x20 , 中心点也是 0, 分辨率是 0.05 m 的地图. 当然, 你会发现 LRFMapDrawer 是啥?  我另外写了专门画地图的程序, 虽然有参考别人的 (ExplorerSim), 但是我有做过修改, 如下:

1.  using System;
2.  using System.Collections.Generic;
3.  using System.ComponentModel;
4.  using Microsoft.Ccr.Core;
5.  using Microsoft.Dss.Core.Attributes;
6.  using sicklrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;
7.  using System.Drawing;
8.  using System.Drawing.Imaging;
9.  using System.Runtime.InteropServices;
10. 
    
11.  namespace LRFDrive
12.  {
13.      public class LRFMapDrawer
14.      {
15.          /// <summary>
16.          /// 0 ~ 127 means occupied, 129 ~ 255 means Vacant , 128 means unknown
17.          /// </summary>
18.          public static byte UnknownMapValue = 128;
19. 
    
20. public static MapBlockInfo CreateMapBlock(double top, double left, double width, double height, double resolution)
21.          {
22.              int resw = (int)Math.Ceiling(width / resolution);
23.              int resh = (int)Math.Ceiling(height / resolution);
24.              byte[,] mapdata = new byte[resw, resh];
25. 
    
26.              for (int x = 0; x < resw; x++)
27.                  for (int y = 0; y < resh; y++)
28.                      mapdata[x, y] = UnknownMapValue;
29. 
    
30.              return new MapBlockInfo()
31.              {
32.                  Top = top,
33.                  Left = left,
34.                  Width = width,
35.                  Height = height,
36.                  Resolution = resolution,
37.                  MapData = mapdata,
38.                  MapDataHeightMax = resh,
39.                  MapDataWidthMax = resw,
40.              };
41.          }
42. 
    
43.          public static void DrawMap(sicklrf.State lrfdata, MapBlockInfo map, DrivePos pos)
44.          {
45.              double currentangle = pos.Direction * 180.0 / Math.PI;
46.              double angle = currentangle + ((double)lrfdata.AngularRange)/2;
47.              foreach (int len in lrfdata.DistanceMeasurements)
48.              {
49.                  double radians = angle * Math.PI / 180;
50.                  double length = len * 0.001;
51.                  bool IsHitted = (len < 8000);
52. 
    
53.                  double dx = length * Math.Cos(radians);
54.                  double dy = length * Math.Sin(radians);
55.                  double EndPointX = pos.X + dx;
56.                  double EndPointY = pos.Y + dy;
57.                  double distance = Math.Sqrt(dx * dx + dy * dy);
58. 
    
59.                  int step = (int)Math.Ceiling(distance/map.Resolution);
60.                  for (int i = 0; i < step; i++)
61.                  {
62.                      double dstep = (double)i / (double)step;
63.                      SetMapValue(map, pos.X + dx * dstep, pos.Y + dy * dstep, 255);
64.                  }
65. 
    
66.                  if (IsHitted)
67.                  {
68.                      SetMapValue(map, EndPointX, EndPointY, 0);
69.                  }
70. 
    
71. 
    
72.                  angle -= lrfdata.AngularResolution;
73.              }
74.          }
75. 
    
76.          private static void SetMapValue(MapBlockInfo map, double x, double y, byte value)
77.          {
78.             int px = (int)Math.Floor((x - map.Left) / map.Resolution);
79.             int py = (int)Math.Floor((y - map.Top) / map.Resolution);
80.             if ((px >= 0) && (px < map.MapDataWidthMax) && (py >= 0) && (py < map.MapDataHeightMax))
81.                 map.MapData[px, py] = value;
82.         }
83. 
    
84. 
    
85.          public static Bitmap CreateBitmap(MapBlockInfo map)
86.          {         Bitmap bmp = new Bitmap(map.MapDataWidthMax, map.MapDataHeightMax, PixelFormat.Format24bppRgb);
87.              BitmapData bmpdata = bmp.LockBits(new Rectangle(0, 0, bmp.Width, bmp.Height),
88.              ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
89. 
    
90.              byte[] data = new byte[bmpdata.Stride * bmp.Height];
91. 
    
92.              int k = 0;
93.              int linestep = 0;
94.              for (int y = 0; y < bmp.Height ; y++)
95.              {
96.                  k = linestep;
97.                  for (int x = 0; x < bmp.Width; x++)
98.                  {
99.                      data[k++] = map.MapData[x, y];
100.                      data[k++] = map.MapData[x, y];
101.                      data[k++] = map.MapData[x, y];
102.                  }
103.                  linestep += bmpdata.Stride;
104.              }
105. 
     
106.              Marshal.Copy(data, 0, bmpdata.Scan0, data.Length);
107. 
     
108.              bmp.UnlockBits(bmpdata);
109.              return bmp;
110.          }
111.      }
112.  }

 

我稍微解释一下地图是怎么画的, 首先根据分辨率, 把地图切为小方块, 每一个方块用 byte 来代表 (byte=128 代表未知, 0~127表示有东西占住, 129~255 表示空的).

当收到 LRF 传来的资料, 就根据自身的位置以及面向的角度, 画出前方的状态.

最后还有一个把地图画成 Bitmap 的函式.

一开始我提到要改架构, 所以现在我们新增了几个 Update DSSP 宣告如下:

1.   /// <summary>
2.   /// LRFDrive main operations port
3.   /// </summary>
4.  [ServicePort]   
5.  public class LRFDriveOperations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, Subscribe, UpdateDrive, UpdateLRF>
6.  {
7.  }
8. 
   
9.  public class UpdateDrive : Update<drive.DriveDifferentialTwoWheelState, PortSet<DefaultUpdateResponseType, Fault>> { }
10. 
    
11.  public class UpdateLRF : Update<sicklrf.State, PortSet<DefaultUpdateResponseType, Fault>> { }

 

好了, 我们可以把 LRFStatus Form 改为加上一个地图的 Picture (位置随你放, 参数自己定喜欢的, 我把名称定为 picmap), 我还多放了一个 ToolStripStatus, 里面放了 Label 用来表示状态, 然后 LRFStatus Form 新增的 code 如下:

1.   public void UpdateMap(MapBlockInfo map)
2.   {
3.       picmap.Image = LRFMapDrawer.CreateBitmap(map);
4.   }
5. 
   
6.   public void UpdatePosInfo(DrivePos pos)
7.   {
8.       tsLabelPosition.Text = string.Format("X:{0}, Y:{1}, Dir:{2} [{3}]",
9.       pos.X, pos.Y, pos.Direction, pos.TimeStamp.ToString("hh:mm:ss.fff"));
10.   }

 

最后, 我们要来更改 Start() , code 如下:

1.   /// <summary>
2.   /// Service start
3.   /// </summary>
4.   protected override void Start()
5.   {
6. 
   
7.       //
8.       // Add service specific initialization here
9.       //
10. 
    
11.       base.Start();
12. 
    
13.       WinFormsServicePort.Post(new RunForm(() =>
14.           {
15.               LRFForm = new LRFStatus();
16.               return LRFForm;
17.           }));
18. 
    
19.       // Subscribe Notification Ports
20.       _sickLRFServicePort.Subscribe(_laserNotify);
21.       _driveDifferentialTwoWheelPort.Subscribe(_driveNotify);
22. 
    
23.       // Activate reciver.
24.       Activate(Arbiter.Receive<sicklrf.Replace>(true, _laserNotify, replace => _mainPort.Post(new UpdateLRF() {Body = replace.Body })));
25.       Activate(Arbiter.Receive<drive.Update>(true, _driveNotify, update => _mainPort.Post(new UpdateDrive() { Body = update.Body })));
26.   }

 

你可以发现到, 之前是收到通知, 就做事, 现在因为我们要做的事情是要改变自身状态 (state), 所以最好是透过 Post 一个变更讯息给自己, 这样才会在 CCR 的要求下维持数据的正确性.

Handler 的函式如下:

1.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
2.   public IEnumerator<ITask> UpdateLRFHandler(UpdateLRF update)
3.   {
4.       _state.LrfState = update.Body;
5.       UpdateLRFImage();
6.       LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
7.       UpdateMap();
8.       UpdatePosInfo();
9.       update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
10.       yield break;
11.   }
12. 
    
13.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
14.   public IEnumerator<ITask> UpdateDriveHandler(UpdateDrive update)
15.   {
16.       _state.DriveState = update.Body;
17.       update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
18.       yield break;
19.   }
20. 
    
21.   private void UpdateMap()
22.   {
23.       WinFormsServicePort.FormInvoke(() =>
24.       {
25.           LRFForm.UpdateMap(_state.Map);
26.       });
27.   }
28. 
    
29.   private void UpdatePosInfo()
30.   {
31.       WinFormsServicePort.FormInvoke(() =>
32.       {
33.           LRFForm.UpdatePosInfo(_state.CurrntPosition);
34.       });
35.   }
36. 
    
37.   private void UpdateLRFImage()
38.   {
39.       WinFormsServicePort.FormInvoke(() =>
40.       {
41.           LRFForm.ReplaceLaserData(_state.LrfState);
42.       });
43. 
    
44.   }

 

比较多的东西是在收到 LRF 变更数据的时候做了很多事情.

按照 code , 就是先把 LRF 的数据存到自己的状态当中, 然后画 LRF , 画地图, 把地图画出来.
恩, 今天都是一堆 code...

放张最后执行的地图结果上来:

 

你觉得上下两张图, 有没有差别呢?

(如果你试着去操控车子...你就知道, 定位不正确会画出啥鸟图, 天啊, 室内要何时才有精准的 GPS ?)

让我们继续一下章教程：

《Robotics Studio学习教程：第十五天——继续利用P3DX画地图》