Life inside Microsoft Robotics

CCR Utilities Part 1

PollRobots — Wed, 23 Jul 2008 09:48:00 GMT

Something that has cropped up a few times — certainly enough times for me to want to do something about it — is the lack of some basic utilities in CCR, not things that necessarily need to find their way into the CCR package, but more some small utility classes that should be in any CCR developers back pocket when they need them.

The first one that I will address is logging. A number of times I've been shown some code and asked "why is my code not as fast as I would like?", and the core algorithm and implementation is just fine, but scattered around the code are lines like…

Log(string.Format("{0}: {1}: Starting New Task", Thread.CurrentThread.
    ManagedThreadId, DateTime.UtcNow));

Which is entirely reasonable, a little investigation into the implementation of Log can shows the real problem

static object _logLock = new object();

static void Log(string msg)
{
    lock(_logLock)
    {
        using (var stream = new StreamWriter("log.txt", true))
        {
            stream.WriteLine(msg);
        }
    }
}

There are two big problems here, and they go hand in hand, one is opening and closing a file everytime a message needs to be logged, and the second, probably introduced by the first decision is to call lock. This of course, ensures that the access to the file is safe, but at the cost of introducing lock contention. Potentially every thread in the dispatcher can be stuck in the one place.

Sometimes on the robotics team we don't realise that this is an issue, in DSS if you want to log there are convenient helper functions on the DsspServiceBase, we forget that CCR doesn't have these.

But CCR is all about solving this problem! A very simple class can encapsulate most basic logging requirements. All we need to do is post messages to a port, and have write them out in an exclusive handler…

We start by creating a small CCR service. This is a common CCR pattern where a class is created to perform some task, but its creator uses a factory function (a static public method commonly called Create) which returns the PortSet that it can communicate with the service on, rather than a classic reference to the class.

The Service

public sealed class StreamLogger : CcrServiceBase
{
    StreamLoggerOperations _port = new StreamLoggerOperations();
    Port<EmptyValue> _tokenPort = new Port<EmptyValue>();
    StreamWriter _writer;

    private StreamLogger(StreamWriter writer, DispatcherQueue taskQueue)
        : base(taskQueue)
    {
        _writer = writer;
    }

In this case the Create method takes the StreamWriter on which logging is to occur, and a DispatcherQueue to use for its own arbiters. Create returns a StreamLoggerOperations object, which we'll look at below

    public static StreamLoggerOperations Create(StreamWriter writer, 
    DispatcherQueue taskQueue)
    {
        if (writer == null)
        {
            throw new ArgumentNullException("writer");
        }
        if (taskQueue == null)
        {
            throw new ArgumentNullException("taskQueue");
        }

        var logger = new StreamLogger(writer, taskQueue);

        logger.Init();

        return logger._port;
    }

Initialization shows us the only "trick" to this. I could have used an Interleave, and defined one Receiver in the Teardown group (for shutdown) and one Receiver in the Exclusive group (for logging messages). That's a great approach, but here I'm doing something a little different which will give me a slightly more explicit behavior on shutdown.

You may have noticed above that one of the private fields of the service as _tokenPort, what I am doing is creating a Join over that token port and the string port within the main operations portset. This allows me (by being careful to ensure that only one message is on the token port at a time) to control the way in which string messages posted to the operations port are handled. Each time a the Join fires (when there is at least one message on each port in the Join) that removes the only message on the token port, and a new message is only posted when the handler completes.

Of course, in order to get the ball rolling I have to post one token onto the token port.

    private void Init()
    {
        Activate<ITask>(
            Arbiter.JoinedReceive<string, EmptyValue>(true, _port, _tokenPort, 
    OnMsg),
            Arbiter.ReceiveWithIterator<Shutdown>(false, _port, OnShutdown)
        );

        //
        // Prime the token port
        //
        _tokenPort.Post(EmptyValue.SharedInstance);
    }

The message handler is simplicity itself, Only two things to note here,

If the message is null or empty, I treat this as a command to flush the writer.
The entire functionality of this handler is wrapped in a try...finally construct, and it is in the finally that the new token is posted, so regardless of what happens in handler there is always a token available to that the join can fire for the next message.

    private void OnMsg(string msg, EmptyValue token)
    {
        try
        {
            if (string.IsNullOrEmpty(msg))
            {
                _writer.Flush();
            }
            else
            {
                _writer.WriteLine(msg);
            }
        }
        finally
        {
            _tokenPort.Post(token);
        }
    }

The only other handler is for Shutdown. This is were reason for the token port becomes apparent. The first thing that the handler does is yield to a receiver on the token port, this does two things, firstly it waits for any message currently being logged to be completed and, secondly it prevents any more message from being logged.

Once the main logging handler is starved, the shutdown handler then removes any messages currently waiting, closes the writer and reports success back to its caller.

    private IEnumerator<ITask> OnShutdown(Shutdown shutdown)
    {
        try
        {
            //
            // steal the serialization token, this stops the OnMsg handler from
            // ever being called again.
            //
            yield return Arbiter.Receive(false, _tokenPort, EmptyHandler);

            //
            // drain the currently outstanding messages, any messages posted
            // after this point will be discarded.
            //
            DrainMsgPort();

            _writer.Flush();
            _writer.Close();
        }
        finally
        {
            shutdown.ResultPort.Post(SuccessResult.Instance);
        }
    }

This drains messages from the port, only draining the number that were on the port when the function was first called, it is easy to see how not having this kind of limit can create code that may not terminate.

    private void DrainMsgPort()
    {
        //
        // drain the number of messages that were available at the start of this 
        // method, anything that arrives later is left for the GC, otherwise
        // this may never terminate.
        //
        var max = _port.P0.ItemCount;
        for (int count = 0; count < max; count++)
        {
            var msg = _port.Test<string>();
            if (!string.IsNullOrEmpty(msg))
            {
                _writer.WriteLine(msg);
            }
        }
    }
}

The Operations Port

The operations port is every bit as important as the service code, it is the interface that you use to communicate with the service.

At its most basic level the port is merely derived from PortSet<string, Shutdown> and that is all that is needed, but adding some helper functions makes it enormously more useable

public sealed class StreamLoggerOperations : PortSet<string, Shutdown>
{
    public void Log(string msg)
    {
        Post(msg);
    }

    public void Log(string format, params object[] args)
    {
        Post(string.Format(format, args));
    }

    public void Log(IFormatProvider provider, string format, params object[] 
    args)
    {
        Post(string.Format(provider, format, args));
    }

    public void Flush()
    {
        Post(string.Empty);
    }

    public SuccessFailurePort Shutdown()
    {
        var s = new Shutdown();

        Post(s);

        return s.ResultPort;
    }
}

Using CCR with ASP.NET

PollRobots — Tue, 10 Jun 2008 04:42:00 GMT

Disclaimer: While I should be somewhat knowledgable about CCR, I can claim no great familiarity with ASP.NET, so if my approach is a little off, please let me know.

How to best use CCR within ASP.NET is something that has come up a number of times. Certainly the powerful concurrency and coordination tools that CCR provides make this an obvious question.

ASP.NET provides a mechanism to run asynchronous tasks, complete with timeout support. When the asynchronous tasks all register completion the page finishes the prerender stage. To make this work with CCR I created an adapter which presents itself to ASP.NET as a classic .Net async task - using the asynchronous programming model (APM), but presents itself to the CCR programmer as a SuccessFailurePort to post a message to when async processing is complete.

A Simple Example

So, to plunge right in, here is somewhat unrealistic, but simple, example...

void Page_Load(object source, EventArgs e)
{
    TaskQueue = Global.TaskQueue;
    
    var asyncPort = base.StartAsyncTask();
    
    var resultPort = new SuccessFailurePort();
    int count = 10;
    for (int i = 0; i < count; i++)
    {
        SpawnIterator("http://wwww.microsoft.com", resultPort, DownloadUrl);
    }
    
    Activate(
        Arbiter.MultipleItemReceive(resultPort,
            count,
            (successes, exceptions) => asyncPort.Post(SuccessResult.Instance)
        )
    );
}

IEnumerator<ITask> DownloadUrl(string url, SuccessFailurePort resultPort)
{
    // Async processing to download from url.

    resultPort.Post(SuccessResult.Instance);
    yield break;
}

Breaking this down, there are several things in the preceding code snippet that aren't available in a typical ASP.NET page, what I haven't shown you (yet!) is that the page is derived from a class CcrPage which gives the page the functionality of the CCR class CcrServiceBase and a couple of extras that are specific to using CCR with ASP.NET.

I start by setting TaskQueue = Global.TaskQueue; It is essential to set the TaskQueue property any time you want to use CCR primitives in the page. I usually set it in Page_Load(), that way I always know it is set...
Where do I get Global.TaskQueue from? It is most efficient to associate the CCR Dispatcher and DispatcherQueues with the Application, rather than the page. So I have created a class, CcrHttpApplication,that inherits from HttpApplication. More on this in a bit.
I call base.StartAsyncTask(); – this is implemented by the base class CcrPage and internally calls the RegisterAsyncTask method, which lets ASP.NET know that this page is doing asynchronous processing. It returns a SuccessFailurePort which I post to when all the asynchronous processing is complete. This in turn lets the ASP.NET infrastructure know that the async processing is finished.
I use CCR primitives like SpawnIterator() and Activate(), these again are implemented in CcrPage

The iterator method DownloadUrl asynchronously downloads a page from the supplied URL and, when it has completed, posts a result to the supplied port. Note that in Page_Load() I activate a MultiItemReceive task that will execute when every instance of DownloadUrl has completed. This is a typical scatter-gather pattern that is so easy to implement using CCR.

When all the DownloadUrl calls have posted a result, that task then posts to the SuccessFailurePort initially created by the call to StartAsyncTask();

A Simple Asynchronous Task

As a small diversion, and for completeness sake, here is the actual code for DownloadUrl(). Feel free to skip ahead the CcrPage implementation if this is uninteresting...

IEnumerator<ITask> DownloadUrl(string url, SuccessFailurePort resultPort)
{
    IAsyncResult result = null;
    var completion = new Port<IAsyncResult>();

    var request = WebRequest.Create(new Uri(url));
    request.BeginGetResponse(completion.Post, null);

    yield return Arbiter.Choice(
        Arbiter.Receive(false, completion, ar => result = ar),
        OnAsyncTimeout()
    );

This demonstrates a useful CCR trick when working with APM. The delegate for the Begin() method is the Post method on a port of type Port<IAsyncResult>, this allows me to yield until the method has completed. In this example I'm using a Choice and in the second branch waiting for a timeout. The OnAsyncTimeout() method in CcrPage will execute if the page timeout fires. This allows me to prevent runaway async processing.

So far the code has initiated a request for a web page, and is waiting for it to complete, in which case result will be set the the IAsyncResult, or for the default page timeout.

    if (result == null)
    {
        // page timeout occurred...
        resultPort.Post(new TimeoutException());
        request.Abort();
        yield break;
    }

This has just checked for a timeout. If a timeout occured, this signals an exception back to the async port, aborts the web request and terminates the iterator. All being well, however, we continue below...

    WebResponse response = null;

    try
    {
        response = request.EndGetResponse(result);
    }
    catch (Exception exception)
    {
        resultPort.Post(exception);
        yield break;
    }

This now uses the IAsyncResult that was posted to the completion port and ends the web request. Assuming that this doesn't throw an exception in its own right, we can now continue below in much the same fashion and, using the APM for reading from a stream, download the entire page before posting a response.

Note: at any point in this processing, we can be interrupted by a timeout, or by a failure in the APM, and still cleanly recover.

    var stream = response.GetResponseStream();

    using (var memory = new MemoryStream())
    {
        var buffer = new byte[4096];
        var read = 0;
        do
        {
            stream.BeginRead(buffer, 0, buffer.Length, completion.Post, null);

            result = null;
            yield return Arbiter.Choice(
                Arbiter.Receive(false, completion, ar => result = ar),
                OnAsyncTimout()
            );

            if (result == null)
            {
                resultPort.Post(new TimeoutException());
                yield break;
            }

            read = stream.EndRead(result);
            memory.Write(buffer, 0, read);

        } while (read == buffer.Length);
    }

    resultPort.Post(SuccessResult.Instance);
}

CcrPage Implementation

The page used above, instead of being derived from System.Web.UI.Page, was derived from CcrPage (coming soon to a Ccr Adapter near you), which looks somewhat like the following (ok, so I've left a lot out to keep the sample smaller, but the core functionality is all here)

public class CcrPage : Page
{
    public CcrPage() { }

    public CcrPage(DispatcherQueue taskQueue)
    {
        TaskQueue = taskQueue;
    }

    protected DispatcherQueue TaskQueue;

Basic class constructors, the infamous TaskQueue field, you'll notice that being used a lot below. Using CCR without a ServiceBase class is, of course, a totally reasonable thing to do, but I find that the helper methods reduce code and increase readability.

The next section is just implementing the helper functionality from CcrServiceBase. If you are familiar with CCR programming this is all pretty self explanatory. For simplicity, I've not included all the overloads here, but all the basics are shown...

    public void Activate<T>(params T[] tasks)
        where T : ITask
    {
        foreach (T task in tasks)
        {
            TaskQueue.Enqueue(task);
        }
    }

    protected void Spawn(Handler handler)
    {
        TaskQueue.Enqueue(new Task(handler));
    }

    protected void Spawn<T0>(T0 t0, Handler<T0> handler)
    {
        TaskQueue.Enqueue(new Task<T0>(t0, handler));
    }

    protected void SpawnIterator<T0>(T0 t0, IteratorHandler<T0> handler)
    {
        TaskQueue.Enqueue(new IterativeTask<T0>(t0, handler));
    }

    protected Port<DateTime> TimeoutPort(int milliseconds)
    {
        return TimeoutPort(new TimeSpan(0, 0, 0, 0, milliseconds));
    }

    protected Port<DateTime> TimeoutPort(TimeSpan timespan)
    {
        Port<DateTime> timeoutPort = new Port<DateTime>();
        TaskQueue.EnqueueTimer(timespan, timeoutPort);
        return timeoutPort;
    }

This is where it starts to get a little more interesting, these are the methods that allow us to play well in the ASP.NET world.

    protected SuccessFailurePort StartAsyncTask()
    {
        SuccessFailurePort resultPort = new SuccessFailurePort();
        Dispatcher.AddCausality(new Causality("StartAsyncTask", resultPort));

        RegisterAsyncTask(
            new PageAsyncTask(
                OnBeginTask,
                OnEndTask,
                OnTimeoutTask,
                resultPort
            )
        );

        return resultPort;
    }

StartAsyncTask wraps the ASP.NET RegisterAsyncTask to allow us to use CCR primitives to control the lifecycle of the async processing that we want to do. The code below should demonstrate and explain how I accomplish this.

Adding a Causality to the dispatcher at this point allows me to catch any unhandled exceptions in the async processing in a clean fashion and report them back using the normal ASP.NET error mechanisms.

    
    protected Port<EmptyValue> AsyncTaskTimeoutPort = new Port<EmptyValue>();

    protected ReceiverTask OnAsyncTimeout(Handler handler)
    {
        return Arbiter.Receive(
            false,
            AsyncTaskTimeoutPort,
            delegate(EmptyValue token)
            {
                AsyncTaskTimeoutPort.Post(token);
                handler();
            }
        );
    }

    protected ReceiverTask OnAsyncTimout()
    {
        return Arbiter.Receive(
            false,
            AsyncTaskTimeoutPort,
            delegate(EmptyValue token)
            {
                AsyncTaskTimeoutPort.Post(token);
            }
        );
    }

These two helper functions make responding to page level timeouts easier in your CCR code. The simpler helper was used in the implementation of DownloadUrl() above.

    class SimpleAsyncResult : IAsyncResult
    {
        ManualResetEvent _event = new ManualResetEvent(false);
        AsyncCallback _cb;
        object _state;
        Exception _exception;

        internal SimpleAsyncResult(AsyncCallback cb, object state)
        {
            _cb = cb;
            _state = state;
        }

        public object AsyncState { get { return _state; } }

        public WaitHandle AsyncWaitHandle { get { return _event; } }

        public bool CompletedSynchronously { get { return false; } }

        public bool IsCompleted
        {
            get { return _event.WaitOne(0, false); }
        }

        internal void Complete(SuccessResult success)
        {
            InternalComplete();
        }

        internal Exception Exception { get { return _exception; } }

        internal void Complete(Exception exception)
        {
            _exception = exception;
            InternalComplete();
        }

        private void InternalComplete()
        {
            _event.Set();
            _cb.Invoke(this);
        }
    }

SimpleAsyncResult is an internal class used to help implement the APM that RegisterAsyncTask expects

    IAsyncResult OnBeginTask(object sender, EventArgs e, AsyncCallback cb, object state)
    {
        if (state == null)
        {
            throw new ArgumentNullException();
        }
        SuccessFailurePort resultPort = state as SuccessFailurePort;
        if (resultPort == null)
        {
            throw new ArgumentException();
        }

        SimpleAsyncResult ar = new SimpleAsyncResult(cb, state);

        Activate(Arbiter.Choice(resultPort, ar.Complete, ar.Complete));

        return ar;
    }

This is called at the start of the async processing and sets up the SimpleAsyncResult instance that is used to manage the lifetime of the async processing.

This ultimately activates a Choice that waits for either a SuccessResult or Exception to be posted to the SuccessFailurePort that was created in StartAsyncTask(). The branches of the choice each call different overloads of the Complete() method on the SimpleAsyncResult object.

    void OnEndTask(IAsyncResult ar)
    {
        if (ar == null)
        {
            throw new ArgumentNullException();
        }
        var sar = ar as SimpleAsyncResult;
        if (sar == null)
        {
            throw new ArgumentException();
        }
        else if (sar.IsCompleted)
        {
            if (sar.Exception != null)
            {
                throw new Exception("Failure during asynchronous processing", sar.Exception);
            }
        }
        else
        {
            throw new InvalidOperationException("OnEndTask called before the asynchronous processing has completed");
        }
    }

This handles all the various termination conditions. In the most common success case this does nothing!

    bool _asyncTimedOut;

    protected bool AsyncOperationTimedOut
    {
        get { return _asyncTimedOut; }
    }

    void OnTimeoutTask(IAsyncResult ar)
    {
        _asyncTimedOut = true;
        AsyncTaskTimeoutPort.Post(EmptyValue.SharedInstance);
    }
}

And finally, the timeout handling. This sets a flag on the class to indicate that there was a timeout and posts to an internal port (used by OnAsyncTimeout above) to inform waiting tasks that there was a timeout.

CcrHttpApplication Implementation

As I mentioned above, it is more useful to associate the Dispatcher and DispatcherQueue with the application than with a page, so I add a Global.asax to my project and instead of deriving it from System.Web.HttpApplication, I derive it from CcrHttpApplication, shown below

public class CcrHttpApplication : HttpApplication
{
    static Dispatcher _dispatcher;
    static DispatcherQueue _taskQueue;
    static object _dispatcherLock = new object();

    static public DispatcherQueue TaskQueue
    {
        get { return _taskQueue; }
    }

With all the basic initialization and property accessors out of the way, we can move onto the functionality of this class...

    protected void Initialize()
    {
        Initialize(0, GetType().Name);
    }

    protected void Initialize(int threadCount)
    {
        Initialize(threadCount, GetType().Name);
    }

    protected void Initialize(int threadCount, string threadPoolName)
    {
        if (_dispatcher == null)
        {
            lock (_dispatcherLock)
            {
                if (_dispatcher == null)
                {
                    _dispatcher = new Dispatcher(threadCount, threadPoolName);
                    _taskQueue = new DispatcherQueue("default", _dispatcher);
                }
            }
        }
    }

This creates a new Dispatcher and default DispatcherQueue. One of the Initialize() overloads should be called from Application_Start()

    protected void Shutdown()
    {
        if (_dispatcher != null)
        {
            lock (_dispatcherLock)
            {
                if (_dispatcher != null)
                {
                    _taskQueue.Dispose();
                    _dispatcher.Dispose();

                    _taskQueue = null;
                    _dispatcher = null;
                }
            }
        }
    }
}

This cleanly disposes any created Dispatcher and DispatcherQueue, and should be called from Application_End()

Pursuit Camera Service (part II)

PollRobots — Fri, 25 Apr 2008 01:41:00 GMT

When I finished the pursuit camera service, I was unhappy with some aspects of it. Fixing them requires implementing the camera as an entity. Most importantly this allows me to update the position every frame and requires the camera to not be the main camera. As a nice consequence implementing this as an entity allows the camera to play well with the record and playback functionality.

Implementing a new camera entity is easy. I just create a new C# file in my existing Pursuit Camera project and add a new class which derives from CameraEntity.

namespace Robotics.PursuitCamera 
{ 
    [DataContract] 
    public class PursuitCameraEntity : CameraEntity 
    {

Configuration

To configure the entity it needs properties that define its behavior

        string _targetName;
        [DataMember, Browsable(true), Description("Name of the entity to track")]
        public string TargetName 
        { 
            get
            {
                return _targetName;
            }
            set
            {
                if (value != _targetName)
                {
                    _targetName = value;
                    FindTarget();
                }
            } 
        }

        [DataMember, Browsable(true), Description("Minimum distance to keep the camera from the entity")]
        public float MinDistance { get; set; }
        
        [DataMember, Browsable(true), Description("Maximum distance to keep the camera from the entity")]
        public float MaxDistance { get; set; }
        
        [DataMember, Browsable(true), Description("Height above ground plane to keep the camera")]
        public float Altitude { get; set; }

These properties clearly define the essential minimum information that the entity needs to track a target. The name of the target is, as before, the entity name to target, the other properties should be fairly obvious.

Note: the setter for TargetName calls a method FindTarget(), I'll come back to that when I discuss the initialization code.

Constructing the Entity

Simulation entities typically get constructed in one of two scenarios.

A simulation scene is being loaded from file and deserialized. In this case a default constructor is called and all the members are populated by the deserialization code.
The entity is being created from the New Entity UI within the simulation window. In this case a parameterized constructor can be called, giving the option of presenting the user with critical configuration choices.

Knowing this, I implement two constructors, and a method to populate all the configuration with sensible default values.

        public PursuitCameraEntity()
        {
            SetDefaults();
        }

        public PursuitCameraEntity(string target)
            : base()
        {
            _targetName = target;
            SetDefaults();      
        }
        
        private void SetDefaults()
        {   
            MinDistance = 4;
            MaxDistance = 6;
            Altitude = 2;
            OcclusionThreshold = 0.5f;
        }

Initializing

Regardless of how the entity is created, it will be initialized before it is used. I use this as the opportunity to find the target entity, and to validate the camera type.

         public override void Initialize(xna.Graphics.GraphicsDevice device, PhysicsEngine physicsEngine)
        {
            base.Initialize(device, physicsEngine);

            if (CameraModel != CameraModelType.FirstPerson)
            {
                CameraModel = CameraModelType.FirstPerson;
            }

            FindTarget();
        }
  
        private void FindTarget()
        {
            if (HasBeenInitialized)
            {
                var query = new VisualEntity();
                query.State.Name = _targetName;

                Activate(
                    Arbiter.Choice(
                        SimulationEngine.GlobalInstancePort.Query(query),
                        success => Target = success.Entity,
                        CcrServiceBase.EmptyHandler
                    )
                );
            }
        }
        
        public VisualEntity Target;

        void Activate<T>(params T[] tasks)
            where T : ITask
        {
            SimulationEngine.GlobalInstance.Activate(tasks);
        }

So, after calling the base Initialize() method, I check that the camera is a first person camera type (which simplifies managing the viewing parameters of the camera) and call FindTarget(), which allows me to post a query to the SimulationEngine that looks for an entity with the name _targetName. If an entity is found then the Target field is set appropriately

Note: querying the simulation engine is an asynchronous process, so I will need to be careful to check that it has completed successfully before I rely on any result.

Tracking the target object

The only thing that remains is to implement the tracking logic on each frame. The Update() method of each VisualEntity is called once per frame. So all I need to do is override that method, and implement the same logic as I did in the service implementation.


        public override void Update(FrameUpdate update)
        {
            if (Target != null)
            {
                // vector to the camera, used for computing the new position
                var entityToCamera = Location - Target.Position;

                var distance = entityToCamera.Length();

                if (distance > 0.1)
                {
                    float scale = 1;

                    // scale the vector if the distance is outside the bounds
                    if (distance > MaxDistance)
                    {
                        scale = MaxDistance / distance;
                    }
                    else if (distance < MinDistance)
                    {
                        scale = MinDistance / distance;
                    }

                    var scaled = xna.Vector3.Multiply(entityToCamera, scale);

                    // set the new camera position, although the altitude will be off.
                    var newCamera = Target.Position + scaled;
                    
                    // constrain the altitude.
                    newCamera.Y = Altitude;

                    base.SetViewParameters(newCamera, Target.Position);
                }
            }
            base.Update(update);
        }

Using the entity

That's all the coding needed to implement this as a simulation entity. Now all that remains is to add this entity to a scene. To do this:

Start a simulation manifest of your choice, as before I'm just using samples\Config\MobileRobots.P3DX.Simulation.manifest.xml
Put the simulator into Edit Mode, by selecting Edit from the Mode menu.
Select New... from the Entity menu
In the New Entity dialog find PursuitCameraEntity in the EntityType list and select it, click OK
When prompted for the target name enter P3DXMotorBase, and click OK
Set the simulator back to Run Mode (Mode -> Run)
Select the new PursuitCamera from the Cameras menu.
Drive the robot around and see the camera following it.
From the File menu, select Save Scene As... to create a new manifest and scene file that now contain the pursuit camera entity

Next steps

Only two things really remain at this point

I'd like to be able to control the entity from a service. This will allow me to dynamically change which object I'm tracking and, by changing the distance, altitude and field of view values, zoom in and out on the target
I still want to do occlusion detection so that the camera can do its best to ensure a clear line of sight to the target entity

Pursuit Camera Service

PollRobots — Sat, 12 Apr 2008 05:35:00 GMT

The problem

Often, when using simulation, it would be convenient to have a camera that automatically tracks a robot (or other entity), so that you can watch what is happening without having to continually adjust the camera.

This is what I set out to write. The aim of the service is to have a camera that follows an entity at a fixed distance and at a fixed height off the ground plane. In a simple way this can be achieved without writing any code at all. Just adding a camera entity as a child to an existing entity does this. This is how the robot camera in the Pioneer 3DX Simulation works. If you run that manifest (/samples/config/MobileRobots.P3DX.Simulation.manifest.xml); you can easily edit the attached camera settings to do this.

Select Mode -> Edit from the Simulation window menu. Then in the entity tree on the top left, expand P3DXMotorBase and select robocam. Then in the properties window on the bottom left, in the Misc group, set the CameraModel to AttachedChild. Then set the Position to 0, 2.5, 5 and the Rotation to -20, 0, 0. Now selecting Mode -> Run from the menu, followed by Camera -> robocam will select that camera view as the current window view. Now as you drive the robot around from the SimpleDashboard, it stays in the center of the simulation window. However, the way the camera moves when the robot turns tends to make me a little nauseous, so I figured that I could implement a pursuit camera quite easily, hence this service…

Another way of illustrating the problem is shown in the following image…

Here the thicker gray line shows the path that a robot is travelling on; the purple path shows how a fixed, or attached, camera would move; and the red path shows how a simple pursuit camera might move. For both cameras the robot is always centered in the cameras view, but the pursuit camera follows a smooth path, whereas the attached camera's path jumps about as the robot rotates on the spot.

Writing the Service

Static Configuration

The first thing that I do when writing a new service is decide what is going to be in the service state. To start with this service just needs to know:

Which entity to follow
The ideal distance of the camera from the entity
The height that the camera should be above the ground plane.

The first of these is best handled as a partner, in common with other simulation services. The other two become part of the service state.

    [DataContract]
    public class PursuitCameraState
    {
        [DataMember, Browsable(true), Description("Distance to 
                  keep the camera from the entity")]
        public float Distance { get; set; }

        [DataMember, Browsable(true), Description("Height above 
                  the ground plane to keep the camera")]
        public float Altitude { get; set; }
    }

I need to be able to configure these, so I specify an InitialStatePartner

    public class PursuitCameraService : DsspServiceBase
    {
        [InitialStatePartner(Optional = true)]
        private PursuitCameraState _state = new PursuitCameraState();

and, because I declare the initial state partner to be optional, I use some (hopefully) sensible defaults in the service's Start() method

        protected override void Start()
        {
            if (_state == null)
            {
                _state = new PursuitCameraState
                {
                    Distance = 5,
                    Altitude = 2
                };
            }

Dynamic Configuration

In order to function, the service needs to know the entity it is following and the camera which it is moving. The entity will be specified using a partner (which I'll get to in a moment), and for now I'm just going to move the main camera.

The best way to get access to the entity is to specify its name as a partner and then to subscribe to the simulation engine. My service will then receive notifications when an entity with that name is added to the scene or deleted from the scene. To do this I add the SimulationEngine as a partner (this is covered in Simulation Tutorial 1 so I won't describe it here), and subscribe to the simulation engine.

My Start() method now contains the following…


        base.Start();

        _simPort = sime.SimulationEngine.GlobalInstancePort;
        _simPort.Subscribe(ServiceInfo.PartnerList, _simNotify);

        base.MainPortInterleave.CombineWith(
            Arbiter.Interleave(
                new TeardownReceiverGroup(),
                new ExclusiveReceiverGroup(
                    Arbiter.ReceiveWithIterator
    <sime.InsertSimulationEntity>(true, _simNotify, 
    OnInsertEntity),
                    Arbiter.Receive
    <sime.DeleteSimulationEntity>(true, _simNotify, 
    OnDeleteEntity)
                ),
                new ConcurrentReceiverGroup()
            )
        );
    }

…because I'm interested in receiving notifications of when the entity is added to the scene I need to receivers for the appropriate notifications, (in this case InsertSimulationEntity and DeleteSimulationEntity) and I also keep a reference to the simulation engine's global instance port as a convenience. Elsewhere in my service class I have declared the following fields…

        sime.SimulationEnginePort _simPort;
        sime.SimulationEnginePort _simNotify = 
                new sime.SimulationEnginePort();

I can now keep track of the lifecycle of the entity that I am tracking by implementing OnInsertEntity and OnDeleteEntity as follows


    sime.VisualEntity _entity;
    sime.CameraEntity _camera;

    IEnumerator<ITask> OnInsertEntity(
        sime.InsertSimulationEntity insert)
    {
        _entity = insert.Body;

        var query = new sime.VisualEntity();
        query.State.Name = "MainCamera";

        yield return Arbiter.Choice(
            _simPort.Query(query),
            success => _camera = success.Entity as sime.CameraEntity,
            failure => LogError("Unable to find camera", failure)
         );
    }

    void OnDeleteEntity(sime.DeleteSimulationEntity delete)
    {
        _entity = null;
        _camera = null;
    }

OnInsertEntity does two things, firstly it stores the entity in the private member _entity, and then it queries the simulation engine to find the main camera (which is conveniently always called "MainCamera"). OnDeleteEntity merely clears these references.

I'm using new C# syntax, so where previously I would have had to write

    sime.VisualEntity query = new sime.VisualEntity();

I now just have


    var query = new sime.VisualEntity();

and where I would have needed


    delegate(sime.QuerySimulationEntityResponseType success)
    {
        _camera = success.Entity as sime.CameraEntity;
    },

I can simply have…

    success => _camera = success.Entity as sime.CameraEntity,

Adjusting the Camera

So far so good, but the service doesn't do anything yet! I now need to adjust the camera in keeping with the height and distance constraints specified in the service state. The simplest way to do this is to create a method on the service that is called periodically, ideally once per frame. To make updates in the simulator on a per-frame basis will require me to write an Entity, I'll cover how to do that another time. For now I chose a simple approach that mostly works!

DSS now has the ability to declare that a service handler should be called periodically, so all I need to do is create a new message type, add it to my service port, and then create a handler in my service.

The new message type is a Submit with an empty body type…

    [DataContract]
    public class TickRequest
    {
    }

    public class Tick : 
        Submit<TickRequest, PortSet<DefaultSubmitResponseType, Fault>>
    {
    }

And the handler for this message is where the real work of this service is done…

        [ServiceHandler(Interval = 0.025)]
        public void OnTick(Tick tick)
        {

The Interval parameter on the ServiceHandler attribute sets an interval in seconds at which the DSS runtime should call this handler. I chose 40 times per second to be in the same order of speed as the frame rate of the simulator.


            if (_entity == null || _camera == null) 
            { 
                tick.ResponsePort.Post(
                    DefaultSubmitResponseType.Instance); 
                return; 
            }

I check that I have references to the entity and the camera, this allows the service to run before the required entity is added to the scene.


            var entityToCamera = _camera.Location - _entity.Position;
            var distance = entityToCamera.Length();

I now just compute the vector from the entity to the camera and find out it's length.


            if (distance > 0.1)
            {

To avoid division by zero, or numerical instability caused by dividing by a very small number, I set an arbitrary threshold (10cm) and don't move the camera if it is very close to the entity.


                var scaled = xna.Vector3.Multiply(entityToCamera, 
                    _state.Distance / distance);
                var newCamera = _entity.Position + scaled;

this scales the vector from the entity to the camera to give it the length required in the service state.

                newCamera.Y = _state.Altitude;

I now force the altitude to be correct, this can cause the distance to be either too short or too long, but over a few frames it will settle into place more or less correctly


                var view = new sime.CameraView
                { 
                    EyePosition = sime.TypeConversion.FromXNA(newCamera), 
                    LookAtPoint = sime.TypeConversion.FromXNA(_entity.Position)
                }; 
                
               _simPort.Update(view);

This then just sets the main camera view to point at the current entity position from the newly computed camera position.

            }
            tick.ResponsePort.Post(DefaultSubmitResponseType.Instance);
        }

The service is now done, now how do I run it…

Running the service

The easiest way to run this is to use the DSS Manifest Editor, or dssme (typically pronounced Dis-Me), to create a manifest. After starting dssme, I import a manifest with an interesting simulation world in it (a great place to start is /samples/config/MobileRobots.P3DX.Simulation.manifest.xml) using File -> Import Manifest.

I then add the SimpleDashboard service from the service list on the left, so that I have some means of driving the robot.

Then I add my new PursuitCamera service and an entry appears in the service list that looks like…

I can now drag the SimulationEngine service onto the SimulationEngine partner link under the PursuitCamera, and then click on the Entity partner and the click on the Select button which appears in the properties to select the entity to follow, if I'm using the simulated pioneer manifest, then I select http://localhost/P3DXMotorBase for the entity.

Now all I need to do is save and run, and the main camera follows the robot as I drive it around!

Next steps

I'm not entirely happy with this service as it stands; it has the following problems…

It's jerky – updating on a timer doesn't work as well as updating every frame would
It always hijacks the main camera – I want to be able to specify which camera to use
The entity may always be in the center of the visual field, but it isn't always visible – I want to do occlusion detection
I would like to be more sophisticated in how the camera moves when the object is too close – specifying minimum and maximum distances would help
This service doesn't play well with the new record/playback functionality – it needs to detect playback and stop interfering

The first of these really requires that this be rewritten to be a new type of camera entity, which also solves the second problem. I'll show how to do that in another post.

A new CTP is here!

PollRobots — Thu, 10 Apr 2008 21:28:28 GMT

A new CTP - A new name - A new version

Microsoft Robotics Developer Studio 2008 CTP April – is now available for download. As always, the downloads page on Microsoft Robotics is a great place to start.

The name has changed; it is now called Microsoft Robotics Developer Studio.

This also represents a new version, Microsoft RDS 2008 contains far too many exciting new features to list here, but do check out what's new.

A new blog…

To introduce myself, I'm Paul Roberts, one of the developers on the Microsoft Robotics team. I'm going to be using this blog to talk about some of what I do on the robotics team; I'll probably pay a little more attention to those features that are close to my heart. I'll also share some services that I have written that might become samples in future releases.

When we are getting ready to release a CTP, all the team members test different parts of the release by writing new services and playing with the new features – some of these remain interesting one day exercises, others become samples and tutorials, still others are about to be dissected in detail on this blog.