Figuring out how the simulation works
First of all I needed to figure out how SimulationTutorial?2 in the simulation tutorials worked. This wasn't very easy because the examples in robotics studio are not very good. The tutorial includes unnecessary functionality (like laser range finders and bumpers and configurability that wasn't needed) that has nothing to do with lesson trying to be taught. The Concurrency and Coordination Runtime (CCR) and Decentralised Software Services (DSS) are complex frameworks and it's never easy to debug a concurrent system from the bottom up. The fact that the tutorials tend not to explain any of the program flow doesn't help with this either.
The approach i took was to break down the tutorial and to remove as much as possible in order that a simple drivable robot is left. This made it easier to understand the interactions of the different services. When I have time I will post up the code for this super simplified version of the tutorial.
Once I had stripped down the tutorial I was able to add to it so that the control form could control a real or simulated NXT.
Using the Wiimote to control a real robot or a simulated robot
The running software is comprised of 2 DSS nodes. The first node runs the following services (addition to the standard services that are launched by a DSS node?:
* Simulation Engine * Simulated differential drive * Simple Dashboard service * SimulationTutorial?2, adding the primitives to the simulation
The second node is launched running the simulated tribot and runs the following services
* NXT Brick * NXT drive * NXT bumper
First of all I'm going to explain what the services running on the first node are and how the work together:
Simulated Diff Drive
The idea of this walk through is to look at how a service exists within a robotics system. I'm hoping that this narrative connects high level concepts to code and will therefore help newcomers understand how all the concepts from the microsoft tutorials relate.
So let's get started.
Simulated Differential Drive Overview
The simulated differential drive service connects to a differential drive entity in the simulation engine and provides a port to the outside world that accepts DriveOperations? such as SetDrivePower?. It routes requests such as SetDrivePower? to the Differential Drive Entity in the simulation engine to effect movement. The reason that this service exists is to protect disinterested clients from the details of connecting and configuring a simulation entity in the simulation engine.
Setting up the connection to the simulation engine
Before any requests can be routed through to the Differential Drive Entity the port on the simulation engine needs to be wired up to the notifications port in the Simulated Differential Drive service. This is done so that requests made to the Simulated Differential Drive flow to the simulation engine and also so that the Simulated Differential Drive can find out when new drive entities have been placed in the simulation engine.
The following diagram shows how the two services, the simaultion engine and the simulated differential drive, are initially wired together.
INSERT DIAGRAM
Looking at the diagram above you can see the port "simEngine", that connects to the Differential Drive entity in the simulation engine. This is wired up via a subscribe request to the "notificationTarget" which is a port of exactly the same type as "simEngine". This is a common pattern used with the Distributed Software Services framework. It forwards all the messages from the port "simEngine" in the simulation engine to the port "notificationTarget" where they are handled by functions attached to the port. In the diagram you can see the arbiter which is the mechanism by which requests are taken off the port and passed to the handler functions.
So to recap there are 5 things that have happened for system to be in the state shown by this diagram:
- Simulated Differential Drive Service is declared
- the variable "simEngine", that resides inside the Simulated Differential Drive service has been created. It is initialised to be the global port from the simulation engine.
- the variable "notificationTarget" is subscribed to _simEngine
- An arbiter is created so that handlers can be wired to the _notificationTarget.
- A handler for entity insertion is created and wired to the arbiter attached to the _notificationTarget. This means that when an entity is added to the simulation the Simulated Differential Drive finds out about it.
So now let's take a look at the code for these 5 things, presented here in the order they happen:
- Declaration of Simulated Differential Drive:
public class SimulatedDifferentialDriveService : Microsoft.Dss.ServiceModel.DsspServiceBase.DsspServiceBase { ... }
All services must derive from DsspServiceBase? for things like Service initialisation and other plumbing.
- The variable _simEngine is declared in the class SimulatedDifferentialDriveService?
public class SimulatedDifferentialDriveService : Microsoft.Dss.ServiceModel.DsspServiceBase.DsspServiceBase { simengine.SimulationEnginePort _simEngine; ... }
and then it is initialised to the global port of the simulation engine
protected override void Start() { _simEngine = simengine.SimulationEngine.GlobalInstancePort; ... }
GlobalInstancePort? is simply a static property of SimulationEngine?:
public static SimulationEnginePort GlobalInstancePort { get; }
- _notificationTarget is created and then subscribes to _simEngine
public class SimulatedDifferentialDriveService : Microsoft.Dss.ServiceModel.DsspServiceBase.DsspServiceBase { simengine.SimulationEnginePort _simEngine; simengine.SimulationEnginePort _notificationTarget; ... }
I've left in the declaration of _simEngine to show that _notificationTarget is of exactly the same type as _simEngine, which is a part of the pattern of forwarding request from a port outside a service to a port inside a service so that they can then be handled.
protected override void Start() { ... _simEngine.Subscribe(ServiceInfo.PartnerList, _notificationTarget); ... }
- An arbiter is created so that handlers can be wired up to _notificationTarget
Activate(new Interleave( new TeardownReceiverGroup ( Arbiter.Receive<simengine.InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime), Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler) ), new ExclusiveReceiverGroup(), new ConcurrentReceiverGroup() ));
An Interleave is a type of arbiter that protects access to internal data structures from concurrent access.
- A handler for entity insertion is created and wired to the arbiter attached to the _notificationTarget
Arbiter.Receive<simengine.InsertSimulationEntity?>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime?),
void InsertEntityNotificationHandlerFirstTime?(simengine.InsertSimulationEntity? ins)
{
...
}
So now you should have a grasp of how the simulated differential drive arrives at it's initial state, waiting for a differential drive to be added to the simulation engine. I'm sure that the details are a bit foggy at the moment. Hopefully I can lift some of the fog by explaining what happens when a differential drive is added to the simulation engine.
Adding a differential drive to the simulation engine
When another service adds a differential drive to the simulation we find out about it. The sequence of events is as follows:
1. The Simulation Engine service receives a request to add a differential drive 2. The Simulation Engine service posts to it's global port 3. The subscription manager takes the request from the global simulation engine port and forwards it to the _notificationTarget port in the simulated differential drive service. 4. The arbiter on _notificationTarget checks to see which requests are running and schedules the execution of the handler function
The diagram below shows this flow:
INSERT DIAGRAM
All of this happens without any intervention from the Simulated Differential Drive Service. As if by magic the first that we know about it is when the handler is fired. So what happens when the handler is fired? Let's take a look at the code:
void InsertEntityNotificationHandlerFirstTime?(simengine.InsertSimulationEntity? ins) {
base.Start();
// Listen on the main port for requests and call the appropriate handler. MainPortInterleave?.CombineWith?(
new Interleave(
new TeardownReceiverGroup?(), new ExclusiveReceiverGroup?(
Arbiter.Receive<simengine.InsertSimulationEntity?>(true, _notificationTarget, InsertEntityNotificationHandler?), Arbiter.Receive<simengine.DeleteSimulationEntity?>(true, _notificationTarget, DeleteEntityNotificationHandler?)
), new ConcurrentReceiverGroup?()
)
);
}
The first thing that happens is that we call another handler function, InsertEntityNotificationHandler?. It's in this handler function that we wire up the diff drive entity from the simulation engine service. So why didn't we ask the arbiter to call this handler directly? A clue to the answer is in the second half of InsertEntityNotificationHandlerFirstTime?. Specifically this part:
// Listen on the main port for requests and call the appropriate handler. MainPortInterleave?.CombineWith?(
new Interleave(
new TeardownReceiverGroup?(), new ExclusiveReceiverGroup?(
Arbiter.Receive<simengine.InsertSimulationEntity?>(true, _notificationTarget, InsertEntityNotificationHandler?), Arbiter.Receive<simengine.DeleteSimulationEntity?>(true, _notificationTarget, DeleteEntityNotificationHandler?)
), new ConcurrentReceiverGroup?()
)
);
MainPortInterleave? is a property on the DsspServiceBase? class. It retrieves the interleaved arbiter that is attached to the main port. In the case of the Simulated Diff Drive the main port is a DriveOperations? port containing things like SetDrivePower?. Then, handlers are added in the exclusive group for entity insertion and deletion.
Let's think this through with me: when the service exists but no entities have been inserted clients on the mainPort can make whichever requests they like and they end up vaporising because there isn't a diff drive entity to add them to yet. We can see this in the code:
[ServiceHandler?(ServiceHandlerBehavior?.Exclusive)] public IEnumerator<ITask> SetPowerHandler?(diffdrive.SetDrivePower? setPower) {
if (_entity == null)
throw new InvalidOperationException?("Simulation entity not registered with service");
...
}
When there is no diff drive entity to manipulate there aren't any concurrency issues to worry about. There is nothing to co-ordinate at all. This even extends to the requests to insert a differential drive. This means that the diff drive insertion / deletion requests can sit on a different arbiter to the DriveOperations? port, mainport. However once you have inserted an entity you want to make sure that the insertion and deletions are co-ordinated with the requests to set drive power properly, which is why the Interleave arbiter is added to the mainports' interleaver arbiter.
Now we'll go back to what the function InsertEntityNotificationHandler?, called from InsertEntityNotificationHandlerFirstTime?:
void InsertEntityNotificationHandlerFirstTime?(simengine.InsertSimulationEntity? ins) {
InsertEntityNotificationHandler?(ins); ...
}
let's take a look at the function itself:
void InsertEntityNotificationHandler?(simengine.InsertSimulationEntity? ins) {
_entity = (simengine.DifferentialDriveEntity?)ins.Body; _entity.ServiceContract? = Contract.Identifier;
// create default state based on the physics entity if(_entity.ChassisShape? != null)
_state.DistanceBetweenWheels? = _entity.ChassisShape?.BoxState?.Dimensions.X;
_state.LeftWheel?.MotorState?.PowerScalingFactor? = _entity.MotorTorqueScaling?; _state.RightWheel?.MotorState?.PowerScalingFactor? = _entity.MotorTorqueScaling?;
}
It's simple! We're simply setting up entity to point to the diff drive from the insertion request and then copying some things about the nature of the diff drive entity into the internal state of the simulated differential drive service.
From now on we can route any requests to set the drive power to "entity". So now everthing is hooked up let's take an overview of the system:
INSERT DIAGRAM
You should be able to see from the diagram the flow of information through the system. The handler for a set SetDrivePower? request is imaginatively named SetPowerHandler?. It looks like this:
[ServiceHandler?(ServiceHandlerBehavior?.Exclusive)] public IEnumerator<ITask> SetPowerHandler?(diffdrive.SetDrivePower? setPower) {
if (_entity == null)
throw new InvalidOperationException?("Simulation entity not registered with service");
// Call simulation entity method for setting wheel torque _entity.SetMotorTorque?(
(float)(setPower.Body.LeftWheelPower?), (float)(setPower.Body.RightWheelPower?));
UpdateStateFromSimulation?(); setPower.ResponsePort?.Post(DefaultUpdateResponseType?.Instance);
// send update notification for entire state _subMgrPort.Post(new submgr.Submit(_state, DsspActions?.UpdateRequest?)); yield break;
}
The bit that actually effects a change in the simulation engine is this line:
// Call simulation entity method for setting wheel torque _entity.SetMotorTorque?(
(float)(setPower.Body.LeftWheelPower?), (float)(setPower.Body.RightWheelPower?));
this call makes it all the way over to the simulation engine where the simulated entity actually moves. This bit may seem a bit like magic, let's leave it at that because at the moment I haven't figured out the mechanism by this works. It works though. Also in this function is an update of the internal state of the differential drive. Potentially something may have happening in the simulation engine that has caused the diff drive state to change without us knowing. For all we know we might have bumped into a giant table, which we certainly want to know about.
All that's left in the handler is to post a response letting the port know what the outcome of the set drive power request was and then to return. Returning from a handler is done by yielding something. In this case there is nothing to yield so we yield break.
SO there you go! hopefully this walk through will help to string together the concepts from the robotics and decentralised software services tutorials. At some point i'll ready the code and post that up too.
