Multiple Simulated Robots

Glossary Item Box

Multiple Simulated Robots

This tutorial builds on the Multiple Simulated Sensors tutorial and the Apartment Scene tutorial, so it is recommended that one completes those tutorials before moving onto this one.

This tutorial teaches you how to:

• Creating the scene
• Partnering with additional services
• Reading and displaying data from the sensors
• Adding the new service to the manifest in DSSMe

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\Advanced\Multiple Simulated Robots

Creating the scene

Before adding the new services to our scene, we will need additional entities so the services can communicate with the simulator. Run the Simulation Empty Project tutorial, load in the apartment scene ("File" -> "Open Scene..." -> "samples\config\Apartment.SimulationEngineState.xml"), and start the EntityUI manifest ("File" -> "Open Manifest..." -> "samples\Config\EntityUI.manifest.xml"). Now add two Motor Base entities with the below services associated as shown below.

Multiple Simulated Robots - Adding the first MotorBase

Multiple Simulated Robots - Adding the second MotorBase

After adding the two entities, save the scene ("File" -> "Save Scene As...").

Partnering with additional services

Our scene contains a variety of services, but for simplicity we will just show how to read data from two different web cameras. One could easily extend the tutorial to read data from all the sensors in the scene, but this would result in a much longer tutorial.

Instead of using DSS New Service, we only need one additional partner so we will just add it manually. Open the Apartment Scene tutorial and add the below code in the "Partners" region.

/// <summary>
/// Port for second webcam
/// </summary>
[Partner("SimulatedWebcam_2", Contract = webcam.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UsePartnerListEntry)]
webcam.WebCamOperations _secondWebcamPort = new webcam.WebCamOperations();

Also rename the "_simulatedWebcamServicePort" member to "_firstWebcamPort" as that is the name that will be used throughout the tutorial. Rebuild the service after making the addition.

Reading and displaying data from the sensors

We will directly use the WinForm from the Apartment Scene tutorial, so creating a new WinForm is not necessary. This tutorial uses a few extra data members for the DSS service, which are listed below. You can remove the existing data members in the "Data Members" region.

Port<DateTime>[] _timers = new[]{ new Port<DateTime>(), new Port<DateTime>() };

Stopwatch _stopWatch = new Stopwatch();

// used to display gradient we compute
ImageProcessingResultForm[] _webcamForms = new ImageProcessingResultForm[2];

Activating the timers and initializing the WinForms

This tutorial shows how to query sensor data from two web cameras, but the implementation could trivially extend to any number of sensors. The first thing we need to do is create the WinForms to display the data and activate two timers.

In the Start() method, add the following code. You should also remove all the other code in the Start() method except for the call to base.Start().

_stopWatch.Start();

var webcamPorts = new[] { _firstWebcamPort, _secondWebcamPort };
for (int idx = 0; idx < _webcamForms.Length; ++idx)
{
    int i = idx;
    WinFormsServicePort.Post(new RunForm(() =>
    {
        _webcamForms[i] = new ImageProcessingResultForm();
        _webcamForms[i].Show();

        Activate(Arbiter.Receive(false, _timers[i], dateTime => UpdateWebcam(webcamPorts[i],
        _webcamForms[i], _timers[i])));
        TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(60), _timers[i]);

        return _webcamForms[i];
    }));
}

Updating sensor data

Updating sensor data is done in a similar fashion to the method used in the Apartment Scene tutorial. Again we use a timer the queries the sensors every 60 milliseconds. This tutorial has a minor addition to improve the accuracy of the query time by using the Stopwatch class.

The full source for querying a webcam and updating the WinForm image is listed below. Just insert these methods into your service's class definition, overwriting all of the existing code in the "Update Image" region.

IEnumerator<ITask> UpdateWebcamHelper(webcam.WebCamOperations webcamPort, ImageProcessingResultForm webcamForm,
    Port<DateTime> timerPort)
{
    Stopwatch stopwatch = new Stopwatch();
    stopwatch.Start();

    yield return new IterativeTask(() => UpdateImage(webcamPort, webcamForm));

    Activate(Arbiter.Receive(false, timerPort, dateTime => UpdateWebcam(webcamPort,
        webcamForm, timerPort)));
    TaskQueue.EnqueueTimer(TimeSpan.FromMilliseconds(Math.Max(1, 60 - stopwatch.ElapsedMilliseconds)), timerPort);

    yield break;
}
void UpdateWebcam(webcam.WebCamOperations webcamPort, ImageProcessingResultForm webcamForm,
    Port<DateTime> timerPort)
{
    SpawnIterator<webcam.WebCamOperations, ImageProcessingResultForm, Port<DateTime>>(
        webcamPort, webcamForm, timerPort, UpdateWebcamHelper);
}

IEnumerator<ITask> UpdateImage(webcam.WebCamOperations webcamPort, ImageProcessingResultForm webcamForm)
{
    long timestamp = _stopWatch.ElapsedMilliseconds;

    byte[] rgbData = null;
    Size size = new Size(0, 0);

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

    TaskQueue.Enqueue(new Task(() =>
        {
            if (rgbData != null)
            {
                ComputeGradient(ref rgbData, size);
                UpdateBitmap(rgbData, size, webcamForm, timestamp);
            }
        }));
}

private void UpdateBitmap(byte[] rgbData, Size size, ImageProcessingResultForm webcamForm,
    long timestamp)
{
    if (webcamForm == null)
        return;

    WinFormsServicePort.Post(new FormInvoke(() =>
    {
        if (timestamp > webcamForm.TimeStamp)
        {
            webcamForm.TimeStamp = timestamp;

            Bitmap bmp = webcamForm.ImageResultBitmap;
            CopyBytesToBitmap(rgbData, size.Width, size.Height, ref bmp);
            if (bmp != webcamForm.ImageResultBitmap)
            {
                webcamForm.UpdateForm(bmp);
            }
            webcamForm.Invalidate(true);
        }
    }));
}

Adding the new service to the manifest in DSSMe

After building the service, start DSSMe and open the manifest that was created when you saved your scene from the simulator. It should look similar to the below picture.

Multiple Simulated Robots - Manifest that was saved from the simulator

Now add the modified Apartment Scene service (by double clicking on the service or dragging it onto the list of services). In the following screenshots, it will be named "MultipleSimulatedRobots", but you likely chose a different name for the service when generating the service with the DSS New Service wizard in the Apartment Scene tutorial. You should see a window similar to the below picture.

Multiple Simulated Robots - Adding the MultipleSimulatedRobots service

Note all the red exclamation points. DSSMe is informing you that the MultipleSimulatedRobots service requires partner services to run correctly. Drag and drop (or copy and paste) existing services into the blocks with the red exclamation points. After you are completed, DSSMe should look similar to the below picture.

Multiple Simulated Robots - Partnering with the webcams

You can also add the simple dashboard service to drive the robot around. Now run the manifest ("Run" -> "Run Manifest..."). The scene should look similar to the below image.

Multiple Simulated Robots - Apartment scene with two webcams

Summary

In this tutorial, you learned how to:

• Creating the scene
• Partnering with additional services
• Reading and displaying data from the sensors
• Adding the new service to the manifest in DSSMe

© 2012 Microsoft Corporation. All Rights Reserved.