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

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

2014年05月03日 Robotics Studio, 文档, 机器人 ⁄ 转载:原文链接 ⁄ 共 2622字 ⁄ 字号 暂无评论 ⁄ 阅读 1,051 次
Robotics Studio学习教程

Robotics Studio学习教程

继之前分享的一篇《Robotics Studio学习教程:第十五天——继续利用P3DX画地图》, 我们将继续分享下一篇《Robotics Studio学习教程:第十六天——利用P3DX自动画地图》,让我继续开始我们学习Visual Programming Language,以及使用Robotics Studio学习开发机器人应用的道路吧。

 

[Robotics Studio] P3DX[IV] - 自动画地图 -- Day16

 

Day15 的地图要自己慢慢画, 但是机器人都是自己 DIY 的啊...所以, 我们有必要再改良一点点.

其实我们只要在画完地图后, 再通知自己进行计算, 然后走下一步, 这样就是自动画地图啰.

所以我们只要在 LRFDriveTypes.cs 当中再加一条 DSSP 宣告 (记得把它放进 LRFDriveOperations 的宣告当中)

  1. public class CalculateNextScan : Update<CalculateNextScanMsg, PortSet<DefaultSubmitResponseType, Fault>> { }

 

然后, Start() 最后加上

  1. Activate(Arbiter.Receive(false, TimeoutPort(1000), time =>
  2. _mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));

 

表示我们在一开始过了一秒后, 开始 ScanMap. 在 ScanMapCommandHandler 当中, Scan 完毕后就通知自己计算下一步, 如下:

  1.    if ((smd.Body.ForwardScan == 0) && (smd.Body.RotateScan == 0))
  2.    {
  3.        LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
  4.        UpdateMap();
  5.        UpdatePosInfo();

  6.        _mainPort.Post(new CalculateNextScan());
  7.    }

 

最后, 就是 CalculateNextScanHandler 啰:

  1.   private bool Pass(int[] data, int begin, int end, int min)
  2.   {
  3.       for (int i = begin; i < end; i++)
  4.       {
  5.           if (data[i] < min)
  6.               return false;
  7.       }
  8.       return true;               
  9.   }

  10.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
  11.   public IEnumerator<ITask> CalculateNextScanHandler(CalculateNextScan scan)
  12.   {
  13.       if (_state.LrfState == null)
  14.           yield break;

  15.       List<ScanMapData> CanDoList = new List<ScanMapData>();

  16.       int mid = _state.LrfState.DistanceMeasurements.Length / 2;

  17.       if (Pass(_state.LrfState.DistanceMeasurements, mid - 15, mid + 15, 2000))
  18.           CanDoList.Add(new ScanMapData() { ForwardScan = 1, RotateScan = 0 });
  19.    if (Pass(_state.LrfState.DistanceMeasurements, _state.LrfState.DistanceMeasurements.Length - 16, _state.LrfState.DistanceMeasurements.Length - 1, 2000))
  20.           CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 90 });

  21.       if (Pass(_state.LrfState.DistanceMeasurements, 1, 15, 2000))
  22.           CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 270 });

  23.       if (CanDoList.Count == 0)
  24.           CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 180 });

  25.       _mainPort.Post(new ScanMapCommand() { Body = CanDoList[0] });

  26.       scan.ResponsePort.Post(DefaultSubmitResponseType.Instance);

  27.       yield break;
  28.   }

 

我采用超级简单的策略, 因为这样就可以漫游了.  如果能够加上 Map 当中的数据, 这样机器人就会变得聪明许多.
但这些就留给有心人啦.

最后, 自动画出来的地图像是这样:

clip_image002

你可以发现, 会有点重迭, 这是因为定位不准确导致.

我们可以考虑利用 Map 当中的资料来做自动修正, 依照我的经验, 系统的 RotateDegree 似乎不是很准确, 会有点误差, 所以才会在旋转过后, 地图就有点误差. 这些就留待日后修正.

最后, 整个 LRFDrive 的原始码可以在这里下载.

 

让我们继续一下章教程:

《Robotics Studio学习教程:第十七天——CCR概况》

fgx

分享到:

Wopus问答

×