iMotoLab

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. 
   
7.        _mainPort.Post(new CalculateNextScan());
8.    }

 

最后, 就是 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. 
    
11.   [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
12.   public IEnumerator<ITask> CalculateNextScanHandler(CalculateNextScan scan)
13.   {
14.       if (_state.LrfState == null)
15.           yield break;
16. 
    
17.       List<ScanMapData> CanDoList = new List<ScanMapData>();
18. 
    
19.       int mid = _state.LrfState.DistanceMeasurements.Length / 2;
20. 
    
21.       if (Pass(_state.LrfState.DistanceMeasurements, mid - 15, mid + 15, 2000))
22.           CanDoList.Add(new ScanMapData() { ForwardScan = 1, RotateScan = 0 });
23.    if (Pass(_state.LrfState.DistanceMeasurements, _state.LrfState.DistanceMeasurements.Length - 16, _state.LrfState.DistanceMeasurements.Length - 1, 2000))
24.           CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 90 });
25. 
    
26.       if (Pass(_state.LrfState.DistanceMeasurements, 1, 15, 2000))
27.           CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 270 });
28. 
    
29.       if (CanDoList.Count == 0)
30.           CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 180 });
31. 
    
32.       _mainPort.Post(new ScanMapCommand() { Body = CanDoList[0] });
33. 
    
34.       scan.ResponsePort.Post(DefaultSubmitResponseType.Instance);
35. 
    
36.       yield break;
37.   }

 

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

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

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

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

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

让我们继续一下章教程：

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