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

Robotics Studio学习教程:第十三天——让我们一起来玩P3DX

2014年04月30日 Robotics Studio, 文档, 机器人 ⁄ 转载:原文链接 ⁄ 共 5845字 ⁄ 字号 暂无评论 ⁄ 阅读 1,115 次
Microsoft Robotics Studio

Microsoft Robotics Studio

继之前分享的一篇《Robotics Studio学习教程:第十二天——利用 DSS 建立虚拟环境(3)》, 我们将继续分享下一篇《Robotics Studio学习教程:第十三天——让我们一起来玩P3DX》,让我继续开始我们学习Visual Programming Language,以及使用Robotics Studio学习开发机器人应用的道路吧。

[Robotics Studio] 开始玩 - 据说价值四万美金的 P3DX -- Day13

LEGO NXT 玩腻了?

没关系, 仿真器就是有这种好处, LEGO 只有碰撞感应, 或是超音波侦测而已, 要玩就玩大一点的, 来玩 Pioneer P3DX 模拟, 这个上面装有 Sick LRF (Laser Range Finder)LMS200 ... 这东东据说也要 6K USD ... 怎么办, 都玩这么贵的虚拟宝物, 胃口都被养大了.

那我们该如何入手来玩这东西, 当然, 先用一个 RDS 已经弄好的 manifest 是最快的.

我们还是可以使用 VPL , 拖一个 GenericDifferentialDrive 来入手的 (微软的打算就是, 通通用我的统一规格, 将来微软还是搞软件, 硬件就交给其他人去搞割喉战吧...哈哈).

这次 manifest 我们选 SimpleVisionSim.Manifest.Xml , 这样我们就可以跑看看了.

clip_image002

这...这家伙要四万美金啊...
不过, 你应该同时注意到, 还多了两个对话框, 一个是 Vision, 一个是 Dashboard , 都还不错玩.

稍微解释 Dashboard 的玩法, 它长的就像这样:

clip_image004

你应该先选择 Remote Node 的 Connect (预设是 localhost), 点选 Connect 以后, 你会看到两个 Device (P3DXMotorBase, P3DXLasterRangeFinder), 此时的 Laser Range Finder 可以点选 connect 看结果, 而左边的 Differential Drive 则需要你双击 (Double Click) P3DXMotorBase 以后, 就会呈现 Motor: On 的状况, 这时你再去点选 Direct Input Device 当中的 Drive, 就可以用那个虚拟的 JoyStick (一个圈圈画十字的那个) 来操控机器车子.

我们就从如何读取 SickLRF 来入手吧, 这个东西比超音波好玩多了.

你可以在网络上找到关于 SickLRF 的相关资料, 像是:

http://www.hizook.com/blog/2008/12/15/sick-laser-rangefinder-lidar-disassembled

http://www.pages.drexel.edu/~kws23/tutorials/sick/sick.html

不过玩过 RDS 以后, 相信你会觉得微软搞出来的东西好像还真的比较好的样子...

如果你对 LMS-200 有点概念, 它其实就像雷达一样, 用扫描的方式来得知周遭物体的距离. 所以我们可以得到一系列的距离数字(从不同角度打出去的雷射所计算出的距离, 以 LMS-200 为例, 你可以取得 180 个距离(有可能会根据分辨率而有不同的数量, 数值是 0 ~ 8000 左右, 单位是 mm (0.001 m)), 分别表示从左边水平线到右边水平线的距离, 所以你知道第 90 个数字(或者中间那个数字) 表示最前头的距离.).

现在我们开始写一个 DSS 来取得 LRF 的数据, 然后显示出来  (就像是 Dashboard 的 Laser Range Finder 那一块)

首先用 VS2008 新开一个 DSS Service 2.0 的专案, 取名为 LRFDrive.

这次我们的 partners 要选两个, 一个是 Sick Laser Range Finder, 另一个则是 Generic Differential Drive (因为我们之后要来操控机器车), 但是我们这次这两个的 CreatePolicy 都要选 UseExisting (采用现有的), 这样可以避免跟 VPL 里面已经加载的 Manifest 相冲, 也可以让它最后(等到所有 Partner Service 完成后)才载入.  但也因为采用 UseExisting, 所以这个 Service 没办法独立从 VS2008 执行. (也许你更改了它的 Manifest 就可以在 VS 2008 内执行, 不过我都在 VPL 里面玩)

接下来, 我们要参考一个 .NET 的 dll - Microsoft.Ccr.Adapters.WinForms (你可以从 .NET 参考中找到它, 不是 RDS 的 bin 目录欧), 顾名思义, 这个是 CCR 提供用来跟 WinForms 界接的接口, 目的就是因为 Windows Form UI 只能在 Form Thread 当中操控 UI 组件的关系.

加完参考后, 我们可以加入一个 Windows Form , 就取名叫做 LRFStatus 好了, 放一个 PictureBox 组件在上面, 把 PictureBox 的 name 改为 pic, SizeMode 改为 StretchImage (自动缩放) , 如下:

clip_image006

现在我们可以到 LRFDriveService class 当中加入这样的宣告:

  1. public LRFStatus LRFForm { getset; }

那这个 LRFForm 就应该在 Start() 函式内被建立出来啰, 是的, 但是要透过 Ccr.Adapters.WinForms, 所以你要加 using Microsoft.Ccr.Adapters.WinForms;
然后 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.   }

 

这样子, 我们回到 VPL 的环境下 (如果你不想重新启动 VPL , 你可以在 VPL 的 View 当中选 Reload Services), 把 LRFDrive 这个 Service 拖到 Diagram 当中,

clip_image008

一旦执行以后, 就会看到我们的 LRFStatus dialog 有跳出来 (最后才跳出来欧, 因为我们是用 UseExisting)

那我们接下来要怎么跟 SickLRF 沟通? 你应该有注意到 Partner service 的 code :

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

我们可以利用 _sickLRFServicePort 来跟 SickLRF 沟通, 采用注册的方式 (Subscribe) , 一旦收到 Replace (状态被全部改变) 就呼叫我们的处理函式, 所以再把 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.   base.Start();

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


  16.    sicklrf.SickLRFOperations _laserNotify = new sicklrf.SickLRFOperations();
  17.    _sickLRFServicePort.Subscribe(_laserNotify);
  18.    Activate(Arbiter.ReceiveWithIterator<sicklrf.Replace>(true, _laserNotify, LrfReplaceHandler));
  19.    }

 

新增加的三行 code , 第一行就是产生一个 Port (严格来说是 PortSet, 可以接受 SickLRF 的所有讯息), 第二行把这个 port 向 SickLRF Service 的 mainport 注册, 第三行是利用 Activate 这个函式, 启动一个 Reciver 的 Task, Reciver 会在 _laserNotify 这个 port 收到 sicklrf.Replace 这样的讯息时, 执行 LrfReplaceHandler, 第一个 true 的参数表示这个 Reciver 的 Task 是一直执行的 (在程序执行期间都是存在的) . 而 LrfReplaceHandler 的 code 如下:

  1.   public IEnumerator<ITask> LrfReplaceHandler(sicklrf.Replace replace)
  2.   {
  3.       WinFormsServicePort.FormInvoke( () =>
  4.           {
  5.               LRFForm.ReplaceLaserData(replace.Body);
  6.           });

  7.       yield break;
  8.   }

终于, 就是收到讯息时, 讯息内容的 Body 就是我们要的 LRF 的信息, 而我们要把它画成图片显示, 所以要在 FromInvoke 当中呼叫 (因为所有 WinForm UI 都要在 UI Thread 当中被执行).

最后, LRFStatus class (那个 Windows Form )当中还要写一个 ReplaceLaserData 的函式, 这个函式内容是抄自 Dashboard 的 source code, 我认为不难理解, 所以就不啰嗦了, 如下:

  1.  public void ReplaceLaserData(sicklrf.State stateType)
  2.  {
  3.      Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
  4.      Graphics g = Graphics.FromImage(bmp);
  5.      g.Clear(Color.LightGray);

  6.      int half = bmp.Height / 2;

  7.      for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
  8.      {
  9.          int range = stateType.DistanceMeasurements[x];
  10.          if (range > 0 && range < 8192)
  11.          {
  12.              int h = bmp.Height * 500 / stateType.DistanceMeasurements[x];
  13.              if (h < 0)
  14.              {
  15.                  h = 0;
  16.              }
  17.              Color col = LinearColor(Color.DarkBlue, Color.LightGray, 0, 8192, range);
  18.              g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
  19.          }
  20.      }
  21.      pic.Image = bmp;
  22.  }

  23.  private Color LinearColor(Color nearColor, Color farColor, int nearLimit, int farLimit, int value)
  24.  {
  25.      if (value <= nearLimit)
  26.      {
  27.          return nearColor;
  28.      }
  29.      else if (value >= farLimit)
  30.      {
  31.          return farColor;
  32.      }

  33.      int span = farLimit - nearLimit;
  34.      int pos = value - nearLimit;

  35.      int r = (nearColor.R * (span - pos) + farColor.R * pos) / span;
  36.      int g = (nearColor.G * (span - pos) + farColor.G * pos) / span;
  37.      int b = (nearColor.B * (span - pos) + farColor.B * pos) / span;

  38.      return Color.FromArgb(r, g, b);
  39.  }

 

于是, 再一次执行 VPL , 你就会发现我们的 LRFStatus 上面的图形跟 Dashboard 的 Laser Range Finder 一样啰. (大小不一样是因为我们用 StretchImage, 我们不需要点选 Connect 才会出现, 因为我们在 Start() 的时候就自己去注册搞定联机了...)

执行的画面如下 (可以想象成一开始透过 LRF 看的画面, 然后透过 dashboard 去玩机器人会更有感觉欧):

clip_image010

让我们继续一下章教程:

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

fgx

分享到:

Wopus问答

×