[Robotics Studio] 繼續修改迷宮, 加上機器人 -- Day26

[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, 就可以開始玩機器人走迷宮了.