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.