继之前分享的一篇《Robotics Studio学习教程:第十五天——继续利用P3DX画地图》, 我们将继续分享下一篇《Robotics Studio学习教程:第十六天——利用P3DX自动画地图》,让我继续开始我们学习Visual Programming Language,以及使用Robotics Studio学习开发机器人应用的道路吧。
[Robotics Studio] P3DX[IV] - 自动画地图 -- Day16
Day15 的地图要自己慢慢画, 但是机器人都是自己 DIY 的啊...所以, 我们有必要再改良一点点.
其实我们只要在画完地图后, 再通知自己进行计算, 然后走下一步, 这样就是自动画地图啰.
所以我们只要在 LRFDriveTypes.cs 当中再加一条 DSSP 宣告 (记得把它放进 LRFDriveOperations 的宣告当中)
- public class CalculateNextScan : Update<CalculateNextScanMsg, PortSet<DefaultSubmitResponseType, Fault>> { }
然后, Start() 最后加上
- Activate(Arbiter.Receive(false, TimeoutPort(1000), time =>
- _mainPort.Post(new ScanMapCommand() { Body = new ScanMapData() { ForwardScan = 0, RotateScan = 0 } })));
表示我们在一开始过了一秒后, 开始 ScanMap. 在 ScanMapCommandHandler 当中, Scan 完毕后就通知自己计算下一步, 如下:
- if ((smd.Body.ForwardScan == 0) && (smd.Body.RotateScan == 0))
- {
- LRFMapDrawer.DrawMap(_state.LrfState, _state.Map, _state.CurrntPosition);
- UpdateMap();
- UpdatePosInfo();
- _mainPort.Post(new CalculateNextScan());
- }
最后, 就是 CalculateNextScanHandler 啰:
- private bool Pass(int[] data, int begin, int end, int min)
- {
- for (int i = begin; i < end; i++)
- {
- if (data[i] < min)
- return false;
- }
- return true;
- }
- [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
- public IEnumerator<ITask> CalculateNextScanHandler(CalculateNextScan scan)
- {
- if (_state.LrfState == null)
- yield break;
- List<ScanMapData> CanDoList = new List<ScanMapData>();
- int mid = _state.LrfState.DistanceMeasurements.Length / 2;
- if (Pass(_state.LrfState.DistanceMeasurements, mid - 15, mid + 15, 2000))
- CanDoList.Add(new ScanMapData() { ForwardScan = 1, RotateScan = 0 });
- if (Pass(_state.LrfState.DistanceMeasurements, _state.LrfState.DistanceMeasurements.Length - 16, _state.LrfState.DistanceMeasurements.Length - 1, 2000))
- CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 90 });
- if (Pass(_state.LrfState.DistanceMeasurements, 1, 15, 2000))
- CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 270 });
- if (CanDoList.Count == 0)
- CanDoList.Add(new ScanMapData() { ForwardScan = 0, RotateScan = 180 });
- _mainPort.Post(new ScanMapCommand() { Body = CanDoList[0] });
- scan.ResponsePort.Post(DefaultSubmitResponseType.Instance);
- yield break;
- }
我采用超级简单的策略, 因为这样就可以漫游了. 如果能够加上 Map 当中的数据, 这样机器人就会变得聪明许多.
但这些就留给有心人啦.最后, 自动画出来的地图像是这样:
你可以发现, 会有点重迭, 这是因为定位不准确导致.
我们可以考虑利用 Map 当中的资料来做自动修正, 依照我的经验, 系统的 RotateDegree 似乎不是很准确, 会有点误差, 所以才会在旋转过后, 地图就有点误差. 这些就留待日后修正.
最后, 整个 LRFDrive 的原始码可以在这里下载.
让我们继续一下章教程:
《Robotics Studio学习教程:第十七天——CCR概况》