Simulated Apartment Scene

Glossary Item Box

Simulated Apartment Scene

This tutorial builds off the Simple Simulated Robot tutorial and shows you how to add the robot to a somewhat visually complex apartment scene.

This tutorial teaches you how to:

• Opening the apartment scene
• Adding a robot and a web camera
• Reading data from a webcam and displaying it in a WinForm

This tutorial is provided in the C# language. You can find the project files for this tutorial at the following location under the Microsoft Robotics Developer Studio installation folder:

Samples\SimulationTutorials\Intermediate\Apartment Scene

Opening the apartment scene

To open the apartment scene in the simulator, simply run any project or service that uses the SimulationEngine (either through Visual Studio or DSSMe). Select "File" -> "Open Scene..." and open the "samples\Config\Apartment.SimulationEngineState.xml" simulation state file.

Apartment Scene Opening - Opening the apartment scene state file

The simulator window will turn black for a second while the scene loads. The scene should look similar to the below image.

Apartment Scene View - The apartment scene

Adding a robot and a web camera

Now we will continue with adding the robot base as we did in the Simple Simulated Robot tutorial. Open the "EntityUI" service by performing the following steps. Select "File" -> "Open Manifest...". Then select the EntityUI manifest located at "samples\config\EntityUI.manifest.xml".

Apartment Scene EntityUI - Opening the EntityUI service's manifest

Now add a motor base entity with a webcam to the scene, with its initial position set to (4, 0, 9) as shown below.

Apartment Scene Motorbase - Adding a MotorBaseWithDrive entity

Now save this scene ("File" -> "Save Scene As...") and choose a name for the scene.

(Optional) Reading data from a webcam and displaying it in a WinForm

Creating the service

The easiest way to create this service is using the DSS New Service Visual Studio project wizard (explained in the "Simulation Empty Project" tutorial). Create a new service that partners with the SimulationEngine, SimulatedWebcam, and SimulatedDifferentialDrive services. The "Partners" tab should look similar to the below image.

Apartment Scene Partners - Adding partners for the new service

Select "UsePartnerListEntry" for the "Creation policy" only for the SimulatedWebcam and SimulatedDifferentialDrive services. This will allow us to add multiple instances of these services in later tutorials. Leave the creation policy of the SimulationEngine to "UseExistingOrCreate" as there will only ever be one instance of the SimulationEngine service.

Creating the WinForm

Select "Project" -> "Add Windows Form..." and type the name "ImageProcessingResultForm.cs". A new windows form will be created. Add a PictureBox on the form through the Toolbox, named "_formPicture". Set its "Dock" property to "Fill" through the Properties window (see picture).

Apartment Scene WinForm - Creating the WinForm

Now select "View" -> "Code" (show below).

Apartment Scene CodeBehind - Viewing the code behind the WinForm

We need to add a few members. Add the below two members to the form.

public Bitmap ImageResultBitmap { get; set; }
public PictureBox PictureBox { get { return _formPicture; } }
public long TimeStamp { get; set; }

In the form's constructor, we will add some initialization logic after the call to InitializeComponent()

ImageResultBitmap = new Bitmap(_formPicture.Width, _formPicture.Height);
_formPicture.Image = ImageResultBitmap;

Finally, add a method to update our WinForm.

internal void UpdateForm(Bitmap bmp)
{
    ImageResultBitmap = bmp;
    this.Size = new Size(bmp.Width, bmp.Height);
    _formPicture.Size = new Size(bmp.Width, bmp.Height);
    _formPicture.Image = ImageResultBitmap;
    this.Invalidate(true);
}

Displaying the data in a WinForm

We will need two extra data members in the service. One is a DateTime CCR port that will allow us to query the SimulatedWebcam every 60 milliseconds so we have an update to date view of the web cam. The other is an instance of the WinForm we added to our project.

Port<DateTime> _dateTimePort = new Port<DateTime>();

// used to display gradient we compute
ImageProcessingResultForm _imageProcessingForm;

Windows Forms uses the Single Threaded Application (STA) programming model, but the CCR/DSS model is highly concurrent. In order to prevent races occurring in WinForms, CCR provides a helper WinForms CCR adapters assembly. You can use this assembly by adding it through the "Project" -> "Add Reference..." menu. The assembly is located at "C:\Program Files\Reference Assemblies\Microsoft\Robotics\[RDS Version]" on 32-bit Windows and "C:\Program Files (x86)\Reference Assemblies\Microsoft\Robotics\[RDS Version]" on 64-bit Windows and is named "Microsoft.Ccr.Adapters.WinForms.dll", where [RDS Version] is the version of RDS you are using (e.g., "v2.3").

Starting a WinForm using the CCR adapters is easy. All you need to do is post a RunForm message to the WinFormsServicePort member of the service (after base.Start() is called in the service).

WinFormsServicePort.Post(new RunForm(() =>
{
    _imageProcessingForm = new ImageProcessingResultForm();
    _imageProcessingForm.Show();
    return _imageProcessingForm;
}));

Where "_imageProcessingForm" is a WinForm that is a member of the ApartmentScene service.

Running code on the Windows Form thread is also easy. Just wrap the code in a FormInvoke message. An example is shown below.

WinFormsServicePort.Post(new FormInvoke(() =>
    {
        Bitmap bmp = _imageProcessingForm.ImageResultBitmap;
        CopyBytesToBitmap(rgbData, size.Width, size.Height, ref bmp);
        if (bmp != _imageProcessingForm.ImageResultBitmap)
        {
            _imageProcessingForm.UpdateForm(bmp);
        }
        _imageProcessingForm.Invalidate(true);
    }));

Querying the SimulatedWebcamService service

In our service's Start() method, we register an iterator method that will be called after 60 milliseconds by the CCR scheduler. The code for this is shown below. The iterator that will be invoked is called "UpdateImage" (defined below). We pass in false to ReceiveWithIterator because we will activate another timer at end of UpdateImage. Passing in true would make this receiver a persistent receiver, but we want it to only receive a single message. This way we prevent a QueryFrame message from being issued while another UpdateImage() is currently executing. Having multiple threads executing UpdateImage at the same time could result in a race updating the bitmap image.

Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateImage));
TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _dateTimePort);

The UpdateImage queries the SimulatedWebcamService service for the latest copy of an image. The CCR method, Arbiter.Choice, is very similar to an if/else construct. If a QueryFrameResponse is returned from QueryFrame(), the first block is run. Otherwise QueryFrame() returns a Fault, in which case the second block is run.

Also, "yield return" will logically wait until either a QueryFrameResponse or a Fault is returned. In other words, you can read the code for UpdateImage as if it were being executed in a synchronous fashion. The full source for UpdateImage is shown below.

IEnumerator<ITask> UpdateImage(DateTime dateTime)
{
    byte[] rgbData = null;
    Size size = new Size(0, 0);

    yield return Arbiter.Choice(_simulatedWebcamServicePort.QueryFrame(),
        success =>
        {
            rgbData = success.Frame;
            size = success.Size;
        },
        failure =>
        {
            LogError(failure.ToException());
        });

    if (rgbData != null)
    {
        ComputeGradient(ref rgbData, size);
        UpdateBitmap(rgbData, size);
    }

    Activate(Arbiter.ReceiveWithIterator(false, _dateTimePort, UpdateImage));
    TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _dateTimePort);
}

There are a few other methods and using statements required to build the tutorial, but the implementation of these can be found in the ApartmentScene project file (ApartmentScene.csproj). They are also listed below.

private void UpdateBitmap(byte[] rgbData, Size size)
{
    if (_imageProcessingForm == null)
        return;

    WinFormsServicePort.Post(new FormInvoke(() =>
        {
            Bitmap bmp = _imageProcessingForm.ImageResultBitmap;
            CopyBytesToBitmap(rgbData, size.Width, size.Height, ref bmp);
            if (bmp != _imageProcessingForm.ImageResultBitmap)
            {
                _imageProcessingForm.UpdateForm(bmp);
            }
            _imageProcessingForm.Invalidate(true);
        }));
}


private void ComputeGradient(ref byte[] rgbData, Size size)
{
    byte[] gradient = new byte[rgbData.Length];
    int[,] mask = new[,]
        {
            {+2, +1, 0},
            {+1, 0, -1},
            {0, -1, -2}
        };
    const int filterSize = 3;
    const int halfFilterSize = filterSize / 2;

    //convolve use simple n^2 method, but this can easily be made 2n
    for (int y = halfFilterSize; y < size.Height - halfFilterSize; y++)
    {
        for (int x = halfFilterSize; x < size.Width - halfFilterSize; x++)
        {
            float result = 0;
            for (int yy = -halfFilterSize; yy <= halfFilterSize; ++yy)
            {
                int y0 = yy + y;
                for (int xx = -halfFilterSize; xx <= halfFilterSize; ++xx)
                {
                    int x0 = xx + x;
                    int k = mask[yy + halfFilterSize, xx + halfFilterSize];
                    int i = 3 * (y0 * size.Width + x0);
                    int r = rgbData[i];
                    int g = rgbData[i + 1];
                    int b = rgbData[i + 2];
                    result += k * (r + g + b) / (3.0f);
                }
            }
            result /= 4.0f; // normalize by max value 
            //the "result*5" makes edges more visible in the image, but is not really necessary
            //  (only nice for display purposes)
            byte byteResult = Clamp(Math.Abs(result * 5.0f), 0.0f, 255.0f);
            int idx = 3 * (y * size.Width + x);
            gradient[idx] = byteResult;
            gradient[idx + 1] = byteResult;
            gradient[idx + 2] = byteResult;
        }
    }

    rgbData = gradient;
}

private byte Clamp(float x, float min, float max)
{
    return (byte)Math.Min(Math.Max(min, x), max);
}


/// <summary>
/// Updates a bitmap from a byte array
/// </summary>
/// <param name="srcData">Should be 32 or 24 bits per pixel (ARGB or RGB format)</param>
/// <param name="srcDataWidth">Width of the image srcData represents</param>
/// <param name="srcDataHeight">Height of the image srcData represents</param>
/// <param name="destBitmap">Bitmap to copy to. Will be recreated if necessary to copy to the array.</param>
private void CopyBytesToBitmap(byte[] srcData, int srcDataWidth, int srcDataHeight, ref Bitmap destBitmap)
{
    int bytesPerPixel = srcData.Length / (srcDataWidth * srcDataHeight);
    if (destBitmap == null
        || destBitmap.Width != srcDataWidth
        || destBitmap.Height != srcDataHeight
        || (destBitmap.PixelFormat == PixelFormat.Format32bppArgb &amp;&amp; bytesPerPixel == 3)
        || (destBitmap.PixelFormat == PixelFormat.Format32bppRgb &amp;&amp; bytesPerPixel == 3)
        || (destBitmap.PixelFormat == PixelFormat.Format24bppRgb &amp;&amp; bytesPerPixel == 4))
    {
        if (bytesPerPixel == 3)
            destBitmap = new Bitmap(srcDataWidth, srcDataHeight, PixelFormat.Format24bppRgb);
        else
            destBitmap = new Bitmap(srcDataWidth, srcDataHeight, PixelFormat.Format32bppRgb);
    }
    BitmapData bmpData = null;
    try
    {
        if (bytesPerPixel == 3)
            bmpData = destBitmap.LockBits(new Rectangle(0, 0, srcDataWidth, srcDataHeight), ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);
        else
            bmpData = destBitmap.LockBits(new Rectangle(0, 0, srcDataWidth, srcDataHeight), ImageLockMode.WriteOnly, PixelFormat.Format32bppRgb);

        Marshal.Copy(srcData, 0, bmpData.Scan0, srcData.Length);
        destBitmap.UnlockBits(bmpData);
    }
    catch (Exception)
    {
    }
}

Additionally, you will need the below using statements.

using Microsoft.Dss.Core;
using System.Drawing;
using Microsoft.Ccr.Adapters.WinForms;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;
using Microsoft.Robotics.Services.Samples;
using Microsoft.Robotics.Simulation.Engine;

Also, remember to change "using drive = Microsoft.Robotics.Services.Simulation.Drive.Proxy;" to "using simulateddrive = Microsoft.Robotics.Services.Simulation.Drive.Proxy;" in ApartmentScene.cs.

Running the tutorial with the WinForm

After you have built the tutorial service, you can use DSSMe to add the service to the manifest you are running. Open DSSMe and open the manifest you were working with (the original tutorial's manifest is "samples\config\ApartmentScene.manifest.xml").

Locate the service under the "Services" box in DSSMe and add the service you just built (the original tutorial's service is named "Apartment Scene"). Add this service to the list of manifests to start up. DSSMe should look similar to the image below (without the red highlight).

Apartment Scene DSSME - Adding the ApartmentScene service through DSSME

After selecting "Run" -> "Run Manifest" from DSSMe, the apartment scene should open in addition to a WinForm with the web camera's image. The simulation sample applies a gradient to the image on the WinForm but you can use the image in other ways as well. For example, you could use the image for vision based navigation by issuing drive requests to the robot based on the image.

Apartment Scene Done - The apartment scene with the ApartmentScene service running in the simulator

Summary

In this tutorial, you learned how to:

• Opening the apartment scene
• Adding a robot and a web camera
• Reading data from a webcam and displaying it in a WinForm

© 2012 Microsoft Corporation. All Rights Reserved.