[Robotics Studio] 繼續修改迷宮, 加上機器人 -- Day26
嗯, 之前的迷宮產生程式有點小 bug, 就是在分割房間的時候, 如果產生的牆壁剛好擋住之前挖開的洞, 這個迷宮就有可能會不通...
所以加上一點小修改來彌補這樣的錯誤, 最後整個 MazeEntity 的程式如下 (我直接把註解寫在 code 當中了):
using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using engine = Microsoft.Robotics.Simulation.Engine.Proxy;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;
namespace MazeGenerator
{
public class MazeEntity : MultiShapeEntity
{
/// <summary>
/// Default constructor
/// </summary>
public MazeEntity() { }
/// <summary>
/// Initialization constructor
/// </summary>
/// <param name="shape"></param>
/// <param name="initialPos"></param>
public MazeEntity(Vector3 initialPos, byte[,] blocks, float mazeheight, float blocksize)
{
for (int x = 0; x < blocks.GetLength(0); x++)
{
for (int y = 0; y < blocks.GetLength(1); y++)
{
if (blocks[x,y] == 0)
continue;
var boxshape = new BoxShape(
new BoxShapeProperties(
100, // mass in kilograms.
new Pose(new Vector3(initialPos.X + x * blocksize, initialPos.Y + mazeheight / 2, initialPos.Z - y*blocksize)), // relative pose
new Vector3(blocksize, mazeheight, blocksize)));
this.BoxShapes.Add(boxshape); // dimensions
}
}
this.State.MassDensity.Mass = blocks.GetLength(0)*blocks.GetLength(1)*100;
}
public static byte[,] GenerateMaze(int width, int height)
{
byte[,] result = new byte[width, height];
Random r = new Random();
for (int x = 0; x < width; x++)
for (int y = 0; y < height; y++)
result[x, y] = (byte)(((x == 0) || (y == 0) || (x == width - 1) || (y == height - 1)) ? 1 : 0);
// dig a hole of left-top, and right-down
result[0, 1] = 0;
result[width - 1, height - 2] = 0;
splitMazeChamber(r, result, 0, 0, width - 1, height - 1);
return result;
}
/// <summary>
/// 將一個房間切割為迷宮, 前提是這個房間四周都是牆壁, 但是會有一些通道, 連到其他房間
/// </summary>
/// <param name="r"></param>
/// <param name="maze"></param>
/// <param name="left"></param>
/// <param name="top"></param>
/// <param name="right"></param>
/// <param name="bottom"></param>
private static void splitMazeChamber(Random r, byte[,] maze, int left, int top, int right, int bottom)
{
int wallX = 0;
if (right - left > 3)
wallX = r.Next(left + 2, right - 1);
int wallY = 0;
if (bottom - top > 3)
wallY = r.Next(top + 2, bottom - 1);
// 停止條件 - 沒得切割房間
if ((wallX <= 0) && (wallY <= 0))
return;
// 建造牆壁來隔開房間
if (wallX > 0)
{
for (int i = top + 1; i < bottom; i++)
maze[wallX, i] = 1;
// 假如這道牆壁剛好蓋在外部的通道, 就拆掉一格牆壁
if (maze[wallX, top] == 0)
maze[wallX, top + 1] = 0;
if (maze[wallX, bottom] == 0)
maze[wallX, bottom - 1] = 0;
}
if (wallY > 0)
{
for (int i = left + 1; i < right; i++)
maze[i, wallY] = 1;
// 假如這道牆壁剛好蓋在外部的通道, 就拆掉一格牆壁
if (maze[left, wallY] == 0)
maze[left + 1, wallY] = 0;
if (maze[right, wallY] == 0)
maze[right - 1, wallY] = 0;
}
// 打通各個房間, 然後繼續切割每個房間
if ((wallX > 0) && (wallY > 0))
{
List<KeyValuePair<int, int>> holes = new List<KeyValuePair<int, int>>();
holes.Add(new KeyValuePair<int, int>(wallX, r.Next(top + 1, wallY - 1)));
holes.Add(new KeyValuePair<int, int>(wallX, r.Next(wallY + 1, bottom - 1)));
holes.Add(new KeyValuePair<int, int>(r.Next(left + 1, wallX - 1), wallY));
holes.Add(new KeyValuePair<int, int>(r.Next(wallX + 1, right - 1), wallY));
holes.RemoveAt(r.Next(0, 4));
holes.ForEach(hole => maze[hole.Key, hole.Value] = 0);
splitMazeChamber(r, maze, left, top, wallX, wallY);
splitMazeChamber(r, maze, wallX, top, right, wallY);
splitMazeChamber(r, maze, left, wallY, wallX, bottom);
splitMazeChamber(r, maze, wallX, wallY, right, bottom);
}
else if (wallX > 0)
{
maze[wallX, r.Next(top + 1, bottom - 1)] = 0;
splitMazeChamber(r, maze, left, top, wallX, bottom);
splitMazeChamber(r, maze, wallX, top, right, bottom);
}
else if (wallY > 0)
{
maze[r.Next(left + 1, right - 1), wallY] = 0;
splitMazeChamber(r, maze, left, top, right, wallY);
splitMazeChamber(r, maze, left, wallY, right, bottom);
}
}
}
}上面的程式就可以產生不會卡死的迷宮了,
接下來, 要放機器人上去, 因為 code 不少, 我就直接把註解寫在 code 當中讓大家看囉:
/// <summary>
/// 產生一個名字後面帶有 Guid, 這樣可以避免與其他名稱重複
/// 因為 VSE 當中的 Entity 名稱是不可以重複的.
/// </summary>
/// <param name="name"></param>
/// <returns></returns>
private string CreateNamePlusGuid(string name)
{
return name + "_" + Guid.NewGuid().ToString();
}
/// <summary>
/// 產生一個虛擬機器人 Pioneer3DX 到 VSE 當中
/// </summary>
/// <param name="position"></param>
/// <param name="angle">面對的角度</param>
private void AddPioneer3DX(Vector3 position, float angle)
{
Pioneer3DX robotBaseEntity = CreatePioneer3DXMotorBase(position);
// 產生虛擬 SickLRF 放入機器人當中
robotBaseEntity.InsertEntity(CreateLaserRangeFinder());
// 產生虛擬的碰撞偵測器放入機器人當中
robotBaseEntity.InsertEntity(CreateBumperArray());
// 產生虛擬的 WebCam 放入機器人當中
robotBaseEntity.InsertEntity(CreateRobotCamera());
// 旋轉角度
robotBaseEntity.State.Pose.Orientation =
TypeConversion.FromXNA(xna.Quaternion.CreateFromAxisAngle(new xna.Vector3(0, 1, 0), (float)((double)angle * Math.PI / (double)180)));
// 將這個 Entity 放入 VSE 當中
SimulationEngine.GlobalInstancePort.Insert(robotBaseEntity);
}
/// <summary>
/// 產生 SimulatedDifferentialDrive 當中已經建立好的 Pioneer3DX 虛擬機器人
/// </summary>
/// <param name="position"></param>
/// <returns></returns>
private Pioneer3DX CreatePioneer3DXMotorBase(Vector3 position)
{
Pioneer3DX robotBaseEntity = new Pioneer3DX(position);
robotBaseEntity.State.Name = CreateNamePlusGuid("P3DXMotorBase");
// 指定 VSE 當中的 Entity 來啟動 SimulatedDifferentialDrive 服務
drive.Contract.CreateService(ConstructorPort, "http://localhost/" + robotBaseEntity.State.Name,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + robotBaseEntity.State.Name)
);
return robotBaseEntity;
}
/// <summary>
/// 產生虛擬的 Sick LRF
/// </summary>
/// <returns></returns>
private LaserRangeFinderEntity CreateLaserRangeFinder()
{
// 產生 Sick LRF Entity, 放在附加物件的中心上方 30 公分 (0.3 公尺) 處
LaserRangeFinderEntity laser = new LaserRangeFinderEntity(
new Pose(new Vector3(0, 0.30f, 0)));
laser.State.Name = CreateNamePlusGuid("P3DXLaserRangeFinder");
// 產生一個虛擬的 LRF 服務, 指定 VSE 當中的 Entity
lrf.Contract.CreateService(
ConstructorPort, "http://localhost/" + laser.State.Name,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + laser.State.Name));
return laser;
}
/// <summary>
/// 產生虛擬的前後碰撞偵測器
/// </summary>
/// <returns></returns>
private BumperArrayEntity CreateBumperArray()
{
// 產生前後的 Bumper Entity
BoxShape frontBumper = new BoxShape(
new BoxShapeProperties("front",
0.001f,
new Pose(new Vector3(0, 0.05f, -0.25f)),
new Vector3(0.40f, 0.03f, 0.03f)
)
);
BoxShape rearBumper = new BoxShape(
new BoxShapeProperties("rear",
0.001f,
new Pose(new Vector3(0, 0.05f, 0.25f)),
new Vector3(0.40f, 0.03f, 0.03f)
)
);
// 物理引擎 (Physics Engine) 必須要指定這個 Entity 啟動碰撞通知, 這樣虛擬服務才會運作正常
frontBumper.State.EnableContactNotifications = true;
rearBumper.State.EnableContactNotifications = true;
BumperArrayEntity
bumperArray = new BumperArrayEntity(frontBumper, rearBumper);
bumperArray.State.Name = CreateNamePlusGuid("P3DXBumpers");
// 指定 Entity 來啟動虛擬服務
bumper.Contract.CreateService(
ConstructorPort, "http://localhost/" + bumperArray.State.Name,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + bumperArray.State.Name));
return bumperArray;
}
/// <summary>
/// 產生虛擬的攝影機
/// </summary>
/// <returns></returns>
private CameraEntity CreateRobotCamera()
{
// 指定解析度
CameraEntity cam = new CameraEntity(320, 240, CameraEntity.CameraModelType.AttachedChild);
cam.State.Name = CreateNamePlusGuid("robocam");
// 放在機器人的上方 50 公分處
cam.State.Pose.Position = new Vector3(0.0f, 0.5f, 0.0f);
// 需要設定這個來配合 Simulated webcam service
cam.IsRealTimeCamera = true;
// 指定 Entity 來啟動虛擬服務
simwebcam.Contract.CreateService(
ConstructorPort, "http://localhost/" + cam.State.Name,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + cam.State.Name)
);
return cam;
}
/// <summary>
/// 產生虛擬的 LEGO-Nxt Tribot 到 VSE 當中
/// </summary>
/// <param name="position"></param>
private void AddLegoNxtRobot(Vector3 position)
{
// 產生 LEGONXTTribot
LegoNXTTribot robotBaseEntity = CreateLegoNxtMotorBase(position);
// 加入前方的碰撞感應器
robotBaseEntity.InsertEntity(CreateLegoNxtBumper());
// 放入 VSE 當中
SimulationEngine.GlobalInstancePort.Insert(robotBaseEntity);
}
/// <summary>
/// 產生 SimulatedDifferentialDrive 當中已經建立好的虛擬 LegoNXTTribot 機器人
/// </summary>
/// <param name="position"></param>
/// <returns></returns>
private LegoNXTTribot CreateLegoNxtMotorBase(Vector3 position)
{
LegoNXTTribot robotBaseEntity = new LegoNXTTribot(position);
robotBaseEntity.State.Name = CreateNamePlusGuid("LegoNXTMotorBase");
// 指定 VSE 當中的 Entity 來啟動 SimulatedDifferentialDrive 服務
drive.Contract.CreateService(
ConstructorPort, "http://localhost/" + robotBaseEntity.State.Name,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + robotBaseEntity.State.Name)
);
return robotBaseEntity;
}
/// <summary>
/// 產生 LEGO Next 前方的碰撞感應器
/// </summary>
/// <returns></returns>
private BumperArrayEntity CreateLegoNxtBumper()
{
BoxShape frontBumper = new BoxShape(
new BoxShapeProperties(
"front", 0.001f, //質量
new Pose(new Vector3(0, 0.063f, -0.09f)), //位置
new Vector3(0.023f, 0.023f, 0.045f)));
// 物理引擎 (Physics Engine) 必須要指定這個 Entity 啟動碰撞通知, 這樣虛擬服務才會運作正常
frontBumper.State.EnableContactNotifications = true;
BumperArrayEntity bumperArray = new BumperArrayEntity(frontBumper);
bumperArray.State.Name = CreateNamePlusGuid("LegoNXTBumpers");
// 指定 Entity 來啟動虛擬服務
bumper.Contract.CreateService(
ConstructorPort, "http://localhost/" + bumperArray.State.Name,
Microsoft.Robotics.Simulation.Partners.CreateEntityPartner(
"http://localhost/" + bumperArray.State.Name));
return bumperArray;
}
以上, 因為使用到不少 Robotics 內建的 simulation , 所以 using 要加上:
using drive = Microsoft.Robotics.Services.Simulation.Drive.Proxy;
using lrf = Microsoft.Robotics.Services.Simulation.Sensors.LaserRangeFinder.Proxy;
using bumper = Microsoft.Robotics.Services.Simulation.Sensors.Bumper.Proxy;
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, 就可以開始玩機器人走迷宮了.