现在的位置: 首页 > 专题 > 机器人 > Robotics Studio > 设计中心 > 文档 > 专题 > 机器人 > 正文

Robotics Studio学习教程:第十四天——利用P3DX画地图

2014年05月01日 Robotics Studio, 文档, 机器人 ⁄ 转载:原文链接 ⁄ 共 10096字 ⁄ 字号 暂无评论 ⁄ 阅读 1,320 次

 

Robotics Studio学习教程

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.  /// <summary>
  8.  /// DriveDifferentialTwoWheel partner
  9.  /// </summary>
  10. [Partner("DriveDifferentialTwoWheel", Contract = drive.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
  11. drive.DriveOperations _driveDifferentialTwoWheelPort = new drive.DriveOperations();
  12. 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.       [DataMember]
  10.       public double X;

  11.       [DataMember]
  12.       public double Y;

  13.       [DataMember]
  14.       public DateTime TimeStamp;

  15.       public DrivePos()
  16.       {
  17.           TimeStamp = DateTime.Now;
  18.           Direction = (double)-90.0 * Math.PI / 180.0;
  19.       }
  20.   }

  21.   [DataContract]
  22.   public class MapBlockInfo
  23.   {
  24.       /// <summary>
  25.       /// Unit mill
  26.       /// </summary>
  27.       [DataMember]
  28.       public double Top;

  29.       [DataMember]
  30.       public double Left;

  31.       [DataMember]
  32.       public double Width;

  33.       [DataMember]
  34.       public double Height;

  35.       /// <summary>
  36.       /// 1 cell (mapdata) size in meter
  37.       /// </summary>
  38.       [DataMember]
  39.       public double Resolution;

  40.       [DataMember]
  41.       public byte[,] MapData;

  42.       [DataMember]
  43.       public int MapDataWidthMax;

  44.       [DataMember]
  45.       public int MapDataHeightMax;
  46.   }

  47.   /// <summary>
  48.   /// LRFDrive state
  49.   /// </summary>
  50.   [DataContract]
  51.   public class LRFDriveState
  52.   {
  53.       [DataMember]
  54.       public DrivePos CurrntPosition;

  55.       [DataMember]
  56.       public drive.DriveDifferentialTwoWheelState DriveState;

  57.       [DataMember]
  58.       public sicklrf.State LrfState;

  59.       [DataMember]
  60.       public MapBlockInfo Map;

  61.       public LRFDriveState()
  62.       {
  63.           CurrntPosition = new DrivePos();
  64.           Map = LRFMapDrawer.CreateMapBlock(-10, -10, 20, 20, 0.05);
  65.       }
  66.   }

 

由上面你可以发现, 我定义了一堆数据型态, 然后在初始化 _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.  namespace LRFDrive
  11.  {
  12.      public class LRFMapDrawer
  13.      {
  14.          /// <summary>
  15.          /// 0 ~ 127 means occupied, 129 ~ 255 means Vacant , 128 means unknown
  16.          /// </summary>
  17.          public static byte UnknownMapValue = 128;

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

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

  26.              return new MapBlockInfo()
  27.              {
  28.                  Top = top,
  29.                  Left = left,
  30.                  Width = width,
  31.                  Height = height,
  32.                  Resolution = resolution,
  33.                  MapData = mapdata,
  34.                  MapDataHeightMax = resh,
  35.                  MapDataWidthMax = resw,
  36.              };
  37.          }

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

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

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

  58.                  if (IsHitted)
  59.                  {
  60.                      SetMapValue(map, EndPointX, EndPointY, 0);
  61.                  }


  62.                  angle -= lrfdata.AngularResolution;
  63.              }
  64.          }

  65.          private static void SetMapValue(MapBlockInfo map, double x, double y, byte value)
  66.          {
  67.             int px = (int)Math.Floor((x - map.Left) / map.Resolution);
  68.             int py = (int)Math.Floor((y - map.Top) / map.Resolution);
  69.             if ((px >= 0) && (px < map.MapDataWidthMax) && (py >= 0) && (py < map.MapDataHeightMax))
  70.                 map.MapData[px, py] = value;
  71.         }


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

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

  77.              int k = 0;
  78.              int linestep = 0;
  79.              for (int y = 0; y < bmp.Height ; y++)
  80.              {
  81.                  k = linestep;
  82.                  for (int x = 0; x < bmp.Width; x++)
  83.                  {
  84.                      data[k++] = map.MapData[x, y];
  85.                      data[k++] = map.MapData[x, y];
  86.                      data[k++] = map.MapData[x, y];
  87.                  }
  88.                  linestep += bmpdata.Stride;
  89.              }

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

  91.              bmp.UnlockBits(bmpdata);
  92.              return bmp;
  93.          }
  94.      }
  95.  }

 

我稍微解释一下地图是怎么画的, 首先根据分辨率, 把地图切为小方块, 每一个方块用 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.  public class UpdateDrive : Update<drive.DriveDifferentialTwoWheelState, PortSet<DefaultUpdateResponseType, Fault>> { }

  9.  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.   public void UpdatePosInfo(DrivePos pos)
  6.   {
  7.       tsLabelPosition.Text = string.Format("X:{0}, Y:{1}, Dir:{2} [{3}]",
  8.       pos.X, pos.Y, pos.Direction, pos.TimeStamp.ToString("hh:mm:ss.fff"));
  9.   }

 

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

  1.   /// <summary>
  2.   /// Service start
  3.   /// </summary>
  4.   protected override void Start()
  5.   {

  6.       //
  7.       // Add service specific initialization here
  8.       //

  9.       base.Start();

  10.       WinFormsServicePort.Post(new RunForm(() =>
  11.           {
  12.               LRFForm = new LRFStatus();
  13.               return LRFForm;
  14.           }));

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

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

 

你可以发现到, 之前是收到通知, 就做事, 现在因为我们要做的事情是要改变自身状态 (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.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
  13.   public IEnumerator<ITask> UpdateDriveHandler(UpdateDrive update)
  14.   {
  15.       _state.DriveState = update.Body;
  16.       update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
  17.       yield break;
  18.   }

  19.   private void UpdateMap()
  20.   {
  21.       WinFormsServicePort.FormInvoke(() =>
  22.       {
  23.           LRFForm.UpdateMap(_state.Map);
  24.       });
  25.   }

  26.   private void UpdatePosInfo()
  27.   {
  28.       WinFormsServicePort.FormInvoke(() =>
  29.       {
  30.           LRFForm.UpdatePosInfo(_state.CurrntPosition);
  31.       });
  32.   }

  33.   private void UpdateLRFImage()
  34.   {
  35.       WinFormsServicePort.FormInvoke(() =>
  36.       {
  37.           LRFForm.ReplaceLaserData(_state.LrfState);
  38.       });

  39.   }

 

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

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

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

clip_image002

 

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

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

 

让我们继续一下章教程:

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

fgx


分享到:

Wopus问答

×