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

Robotics Studio学习教程:第二十六天——让我们在迷宫中加入机器人!

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

Robotics Studio学习教程

继之前分享的一篇《Robotics Studio学习教程:第二十五天——让我们开始建造迷宫吧!》, 我们将继续分享下一篇《Robotics Studio学习教程:第二十六天——让我们在迷宫中加入机器人!》,让我继续开始我们学习Visual Programming Language,以及使用Robotics Studio学习开发机器人应用的道路吧。

 

[Robotics Studio继续修改迷宫, 加上机器人 -- Day26

嗯, 之前的迷宫产生程序有点小 bug, 就是在分割房间的时候, 如果产生的墙壁刚好挡住之前挖开的洞, 这个迷宫就有可能会不通...

所以加上一点小修改来弥补这样的错误, 最后整个 MazeEntity 的程序如下 (我直接把批注写在 code 当中了):

  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 Microsoft.Dss.ServiceModel.Dssp;
  7. using Microsoft.Dss.ServiceModel.DsspServiceBase;
  8. using W3C.Soap;
  9. using submgr = Microsoft.Dss.Services.SubscriptionManager;
  10. using engine = Microsoft.Robotics.Simulation.Engine.Proxy;
  11. using Microsoft.Robotics.Simulation.Engine;
  12. using Microsoft.Robotics.Simulation.Physics;
  13. using Microsoft.Robotics.PhysicalModel;
  14. namespace MazeGenerator
  15. {
  16. public class MazeEntity : MultiShapeEntity
  17. {
  18. /// <summary>
  19. /// Default constructor
  20. /// </summary>
  21. public MazeEntity() { }
  22. /// <summary>
  23. /// Initialization constructor
  24. /// </summary>
  25. /// <param name="shape"></param>
  26. /// <param name="initialPos"></param>
  27. public MazeEntity(Vector3 initialPos, byte[,] blocks, float mazeheight, float blocksize)
  28. {
  29. for (int x = 0; x < blocks.GetLength(0); x++)
  30. {
  31. for (int y = 0; y < blocks.GetLength(1); y++)
  32. {
  33. if (blocks[x,y] == 0)
  34. continue;
  35. var boxshape = new BoxShape(
  36. new BoxShapeProperties(
  37. 100, // mass in kilograms.
  38. new Pose(new Vector3(initialPos.X +
  39. x * blocksize, initialPos.Y +
  40. mazeheight / 2,
  41. initialPos.Z - y*blocksize)), // relative pose
  42. new Vector3(blocksize, mazeheight, blocksize)));
  43. this.BoxShapes.Add(boxshape); // dimensions
  44. }
  45. }
  46. this.State.MassDensity.Mass = blocks.GetLength(0)*blocks.GetLength(1)*100;
  47. }
  48. public static byte[,] GenerateMaze(int width, int height)
  49. {
  50. byte[,] result = new byte[width, height];
  51. Random r = new Random();
  52. for (int x = 0; x < width; x++)
  53. for (int y = 0; y < height; y++)
  54. result[x, y] = (byte)(((x == 0) || (y == 0) || (x == width - 1) || (y == height - 1)) ? 1 : 0);
  55. // dig a hole of left-top, and right-down
  56. result[0, 1] = 0;
  57. result[width - 1, height - 2] = 0;
  58. splitMazeChamber(r, result, 0, 0, width - 1, height - 1);
  59. return result;
  60. }
  61. /// <summary>
  62. /// 将一个房间切割为迷宫, 前提是这个房间四周都是墙壁, 但是会有一些通道, 连到其他房间
  63. /// </summary>
  64. /// <param name="r"></param>
  65. /// <param name="maze"></param>
  66. /// <param name="left"></param>
  67. /// <param name="top"></param>
  68. /// <param name="right"></param>
  69. /// <param name="bottom"></param>
  70. private static void splitMazeChamber(Random r, byte[,]
  71. maze, int left, int top, int right, int bottom)
  72. {
  73. int wallX = 0;
  74. if (right - left > 3)
  75. wallX = r.Next(left + 2, right - 1);
  76. int wallY = 0;
  77. if (bottom - top > 3)
  78. wallY = r.Next(top + 2, bottom - 1);
  79. // 停止条件 - 没得切割房间
  80. if ((wallX <= 0) && (wallY <= 0))
  81. return;
  82. // 建造墙壁来隔开房间
  83. if (wallX > 0)
  84. {
  85. for (int i = top + 1; i < bottom; i++)
  86. maze[wallX, i] = 1;
  87. // 假如这道墙壁刚好盖在外部的通道, 就拆掉一格墙壁
  88. if (maze[wallX, top] == 0)
  89. maze[wallX, top + 1] = 0;
  90. if (maze[wallX, bottom] == 0)
  91. maze[wallX, bottom - 1] = 0;
  92. }
  93. if (wallY > 0)
  94. {
  95. for (int i = left + 1; i < right; i++)
  96. maze[i, wallY] = 1;
  97. // 假如这道墙壁刚好盖在外部的通道, 就拆掉一格墙壁
  98. if (maze[left, wallY] == 0)
  99. maze[left + 1, wallY] = 0;
  100. if (maze[right, wallY] == 0)
  101. maze[right - 1, wallY] = 0;
  102. }
  103. // 打通各个房间, 然后继续切割每个房间
  104. if ((wallX > 0) && (wallY > 0))
  105. {
  106. List<KeyValuePair<int, int>> holes = new List<KeyValuePair<int, int>>();
  107. holes.Add(new KeyValuePair<int, int>(wallX, r.Next(top + 1, wallY - 1)));
  108. holes.Add(new KeyValuePair<int, int>(wallX, r.Next(wallY + 1, bottom - 1)));
  109. holes.Add(new KeyValuePair<int, int>(r.Next(left + 1, wallX - 1), wallY));
  110. holes.Add(new KeyValuePair<int, int>(r.Next(wallX + 1, right - 1), wallY));
  111. holes.RemoveAt(r.Next(0, 4));
  112. holes.ForEach(hole => maze[hole.Key, hole.Value] = 0);
  113. splitMazeChamber(r, maze, left, top, wallX, wallY);
  114. splitMazeChamber(r, maze, wallX, top, right, wallY);
  115. splitMazeChamber(r, maze, left, wallY, wallX, bottom);
  116. splitMazeChamber(r, maze, wallX, wallY, right, bottom);
  117. }
  118. else if (wallX > 0)
  119. {
  120. maze[wallX, r.Next(top + 1, bottom - 1)] = 0;
  121. splitMazeChamber(r, maze, left, top, wallX, bottom);
  122. splitMazeChamber(r, maze, wallX, top, right, bottom);
  123. }
  124. else if (wallY > 0)
  125. {
  126. maze[r.Next(left + 1, right - 1), wallY] = 0;
  127. splitMazeChamber(r, maze, left, top, right, wallY);
  128. splitMazeChamber(r, maze, left, wallY, right, bottom);
  129. }
  130. }
  131. }
  132. }

 

上面的程序就可以产生不会卡死的迷宫了, 接下来, 要放机器人上去, 因为 code 不少, 我就直接把批注写在 code 当中让大家看啰:

  1. /// <summary>
  2. /// 产生一个名字后面带有 Guid, 这样可以避免与其他名称重复
  3. /// 因为 VSE 当中的 Entity 名称是不可以重复的.
  4. /// </summary>
  5. /// <param name="name"></param>
  6. /// <returns></returns>
  7. private string CreateNamePlusGuid(string name)
  8. {
  9. return name + "_" + Guid.NewGuid().ToString();
  10. }
  11. /// <summary>
  12. /// 产生一个虚拟机人 Pioneer3DX 到 VSE 当中
  13. /// </summary>
  14. /// <param name="position"></param>
  15. /// <param name="angle">面对的角度</param>
  16. private void AddPioneer3DX(Vector3 position, float angle)
  17. {
  18. Pioneer3DX robotBaseEntity = CreatePioneer3DXMotorBase(position);
  19. // 产生虚拟 SickLRF 放入机器人当中
  20. robotBaseEntity.InsertEntity(CreateLaserRangeFinder());
  21. // 产生虚拟的碰撞侦测器放入机器人当中
  22. robotBaseEntity.InsertEntity(CreateBumperArray());
  23. // 产生虚拟的 WebCam 放入机器人当中
  24. robotBaseEntity.InsertEntity(CreateRobotCamera());
  25. // 旋转角度
  26. robotBaseEntity.State.Pose.Orientation =
  27. TypeConversion.FromXNA(xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float) ((double)angle * Math.PI / (double)180)));
  28. // 将这个 Entity 放入 VSE 当中
  29. SimulationEngine.GlobalInstancePort.Insert(robotBaseEntity);
  30. }
  31. /// <summary>
  32. /// 产生 SimulatedDifferentialDrive 当中已经建立好的 Pioneer3DX 虚拟机人
  33. /// </summary>
  34. /// <param name="position"></param>
  35. /// <returns></returns>
  36. private Pioneer3DX CreatePioneer3DXMotorBase(Vector3 position)
  37. {
  38. Pioneer3DX robotBaseEntity = new Pioneer3DX(position);
  39. robotBaseEntity.State.Name = CreateNamePlusGuid("P3DXMotorBase");
  40. // 指定 VSE 当中的 Entity 来启动 SimulatedDifferentialDrive 服务
  41. drive.Contract.CreateService(ConstructorPort, "http://localhost/" + robotBaseEntity.State.Name,
  42. Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
  43. "http://localhost/" + robotBaseEntity.State.Name)
  44. );
  45. return robotBaseEntity;
  46. }
  47. /// <summary>
  48. /// 产生虚拟的 Sick LRF
  49. /// </summary>
  50. /// <returns></returns>
  51. private LaserRangeFinderEntity CreateLaserRangeFinder()
  52. {
  53. // 产生 Sick LRF Entity, 放在附加对象的中心上方 30 公分 (0.3 公尺) 处
  54. LaserRangeFinderEntity laser = new LaserRangeFinderEntity(
  55. new Pose(new Vector3(0, 0.30f, 0)));
  56. laser.State.Name = CreateNamePlusGuid("P3DXLaserRangeFinder");
  57. // 产生一个虚拟的 LRF 服务, 指定 VSE 当中的 Entity
  58. lrf.Contract.CreateService(
  59. ConstructorPort, "http://localhost/" + laser.State.Name,
  60. Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
  61. "http://localhost/" + laser.State.Name));
  62. return laser;
  63. }
  64. /// <summary>
  65. /// 产生虚拟的前后碰撞侦测器
  66. /// </summary>
  67. /// <returns></returns>
  68. private BumperArrayEntity CreateBumperArray()
  69. {
  70. // 产生前后的 Bumper Entity
  71. BoxShape frontBumper = new BoxShape(
  72. new BoxShapeProperties("front", 0.001f,
  73. new Pose(new Vector3(0, 0.05f, -0.25f)),
  74. new Vector3(0.40f, 0.03f, 0.03f)
  75. )
  76. );
  77. BoxShape rearBumper = new BoxShape(
  78. new BoxShapeProperties("rear",0.001f,
  79. new Pose(new Vector3(0, 0.05f, 0.25f)),
  80. new Vector3(0.40f, 0.03f, 0.03f)
  81. )
  82. );
  83. // 物理引擎 (Physics Engine) 必须要指定这个 Entity 启动碰撞通知, 这样虚拟服务才会运作正常
  84. frontBumper.State.EnableContactNotifications = true;
  85. rearBumper.State.EnableContactNotifications = true;
  86. BumperArrayEntity bumperArray = new BumperArrayEntity(frontBumper, rearBumper);
  87. bumperArray.State.Name = CreateNamePlusGuid("P3DXBumpers");
  88. // 指定 Entity 来启动虚拟服务
  89. bumper.Contract.CreateService(
  90. ConstructorPort, "http://localhost/" + bumperArray.State.Name,
  91. Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
  92. "http://localhost/" + bumperArray.State.Name));
  93. return bumperArray;
  94. }
  95. /// <summary>
  96. /// 产生虚拟的摄影机
  97. /// </summary>
  98. /// <returns></returns>
  99. private CameraEntity CreateRobotCamera()
  100. {
  101. // 指定分辨率
  102. CameraEntity cam = new CameraEntity(320, 240, CameraEntity.CameraModelType.AttachedChild);
  103. cam.State.Name = CreateNamePlusGuid("robocam");
  104. // 放在机器人的上方 50 公分处
  105. cam.State.Pose.Position = new Vector3(0.0f, 0.5f, 0.0f);
  106. // 需要设定这个来配合 Simulated webcam service
  107. cam.IsRealTimeCamera = true;
  108. // 指定 Entity 来启动虚拟服务
  109. simwebcam.Contract.CreateService(
  110. ConstructorPort, "http://localhost/" + cam.State.Name,
  111. Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
  112. "http://localhost/" + cam.State.Name)
  113. );
  114. return cam;
  115. }
  116. /// <summary>
  117. /// 产生虚拟的 LEGO-Nxt Tribot 到 VSE 当中
  118. /// </summary>
  119. /// <param name="position"></param>
  120. private void AddLegoNxtRobot(Vector3 position)
  121. {
  122. // 产生 LEGONXTTribot
  123. LegoNXTTribot robotBaseEntity = CreateLegoNxtMotorBase(position);
  124. // 加入前方的碰撞传感器
  125. robotBaseEntity.InsertEntity(CreateLegoNxtBumper());
  126. // 放入 VSE 当中
  127. SimulationEngine.GlobalInstancePort.Insert(robotBaseEntity);
  128. }
  129. /// <summary>
  130. /// 产生 SimulatedDifferentialDrive 当中已经建立好的虚拟 LegoNXTTribot 机器人
  131. /// </summary>
  132. /// <param name="position"></param>
  133. /// <returns></returns>
  134. private LegoNXTTribot CreateLegoNxtMotorBase(Vector3 position)
  135. {
  136. LegoNXTTribot robotBaseEntity = new LegoNXTTribot(position);
  137. robotBaseEntity.State.Name = CreateNamePlusGuid("LegoNXTMotorBase");
  138. // 指定 VSE 当中的 Entity 来启动 SimulatedDifferentialDrive 服务
  139. drive.Contract.CreateService(
  140. ConstructorPort, "http://localhost/" + robotBaseEntity.State.Name,
  141. Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
  142. "http://localhost/" + robotBaseEntity.State.Name)
  143. );
  144. return robotBaseEntity;
  145. }
  146. /// <summary>
  147. /// 产生 LEGO Next 前方的碰撞传感器
  148. /// </summary>
  149. /// <returns></returns>
  150. private BumperArrayEntity CreateLegoNxtBumper()
  151. {
  152. BoxShape frontBumper = new BoxShape(
  153. new BoxShapeProperties(
  154. "front", 0.001f, //质量
  155. new Pose(new Vector3(0, 0.063f, -0.09f)), //位置
  156. new Vector3(0.023f, 0.023f, 0.045f)));
  157. // 物理引擎 (Physics Engine) 必须要指定这个 Entity 启动碰撞通知, 这样虚拟服务才会运作正常
  158. frontBumper.State.EnableContactNotifications = true;
  159. BumperArrayEntity bumperArray = new BumperArrayEntity(frontBumper);
  160. bumperArray.State.Name = CreateNamePlusGuid("LegoNXTBumpers");
  161. // 指定 Entity 来启动虚拟服务
  162. bumper.Contract.CreateService(
  163. ConstructorPort, "http://localhost/" + bumperArray.State.Name,
  164. Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
  165. "http://localhost/" + bumperArray.State.Name));
  166. return bumperArray;
  167. }

 

以上, 因为使用到不少 Robotics 内建的 simulation , 所以 using 要加上:

  1. using drive = Microsoft.Robotics.Services.Simulation.Drive.Proxy;
  2. using lrf = Microsoft.Robotics.Services.Simulation.Sensors.LaserRangeFinder.Proxy;
  3. using bumper = Microsoft.Robotics.Services.Simulation.Sensors.Bumper.Proxy;
  4. using simwebcam = Microsoft.Robotics.Services.Simulation.Sensors.SimulatedWebcam.Proxy;

 

当然, 参考的 dll 就是 SimulatedBumper.Y2006.M05.proxy, SimulatedDifferentialDrive.2006.M06.proxy, SimulatedLRF.Y2006.M05.proxy, SimulatedWebcam.Y2006.M09.proxy, 以上都可以在 Robotics Developer Studio 安装目录下面的 bin 当中找到.

但是有一个比较麻烦的 dll 是 Microsoft.Xna.Framework.dll , 它会放在 Windows 目录下的 GAC_32 下面,(以我的计算机为例子是 C:\WINDOWS\assembly\GAC_32\Microsoft.Xna.Framework\2.0.0.0__6d5c3888ef60e27d )这个 microsoft.xna.framework.dll 你可以把它复制出来到 robotics studio 安装目录下, 然后再参考比较方便.

有了以上那么多行的 code, 终于可以加上这行:AddPioneer3DX(new Vector3(1.5f, 0.1f, 0f), -90);然后你就有 maze, 又有机器人啦 (还帮你把角度面对迷宫哩) ...

(事实上我也给了产生 LegoNXT 的 code, 你可以自己加)

简单的做法是把这个 Service 丢给 VPL 启动, 然后你再加上 Dashboard 这个 Service, 就可以开始玩机器人走迷宫了.

 

让我们继续一下章教程:

《Robotics Studio学习教程:第二十七天——在VSE中使用CameraSprite》

fgx

分享到:

Wopus问答

×