Lab Tutorial 7 (C#) - Multirobot Coordination

  • Article

In this lab you will learn to communicate between robots, while examining a decentralized coordination strategy and a centralized coordination strategy. You will also learn how to build a service from the ground up in C#.

You will use two robots. The robots will bump into each another, determine which robot gets to lead, and then the leader will perform some motion while the other robot follows along.

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

 Samples\Courseware\Introductory\Lab7

This tutorial teaches you how to:

  • Create Service
  • Define Types
  • Add Partnerships
  • Bump into other Robot
  • Multirobot Communication and Decentralized Election
  • Follow the Leader

See Also:

  • Try It Out

Prerequisites

Hardware

You will need two robots with front bumpers (or you can simulate these robots).

Software

This tutorial is designed for use with Microsoft Visual C#. You can use:

  • Microsoft Visual C# Express Edition
  • Microsoft Visual Studio Standard, Professional, or Team Edition.

 

You will also need Microsoft Internet Explorer or another conventional web browser.

Step 1: Create the Service

In this lab, we will not use Microsoft Visual Programming Language, writing the service entirely in C#. First, use the tool DssNewService to generate the necessary files and classes.

On the command-line, choose the directory in which you wish to create your new service. Enter the command: dssnewservice /namespace:Robotics /service:Lab7, which specifies the namespace and service name for your new service. Now you should see a directory with the name Lab7; enter this directory. You will see the Visual Studio project files that were generated for you; the two C# source files, defining the types and the service behavior; and the manifest file, which specifies to the DSS infrastructure how to run the service and any dependencies on other services.

Open the file Lab7.sln to begin editing the service.

Step 2: Define the Types

First, we need to add references to the assemblies of the components we'll need (the bumper contact sensor and the differential drive); this will tell Visual Studio to link to these files when building. Right-click on References and choose Add Reference. Choose RoboticsCommon.Proxy (remember, proxies abstract whether the other services are local or remote, so it is good practice to always use the proxy [they are automatically generated for you when you build a service]).

We'll start by defining the necessary types, both for the service state and its messages. Open the file Lab7Types.cs . At the top, add an alias for the differential drive proxy (part of the RoboticsCommon assembly):

 using drive = Microsoft.Robotics.Services.Drive.Proxy;
using System.ComponentModel;

Now find the Lab7State class. Add three properties, as below:

 [DataContract()]
public class Lab7State
{
    bool _initiateDialog;
    [DataMember]
    public bool initiateDialog
    {
        get { return _initiateDialog; }
        set { _initiateDialog = value; }
    }

    int _priority;
    [DataMember, Browsable(false)]
    public int Priority
    {
        get { return _priority; }
        set { _priority = value; }
    }

    private RobotMode _mode = RobotMode.TryingToBump;
    [DataMember, Browsable(false)]
    public RobotMode Mode
    {
        get { return _mode; }
        set { _mode = value; }
    }

}

[DataContract]
public enum RobotMode
{
    TryingToBump,
    Listening,
    Talking,
    Leading,
    Following
}
  • InitiateDialog - Although the robots will be peers, we wish to avoid the complexity of dynamic partner discovery in this tutorial. Hence, we will set one robot to initiate the dialog, although it might not necessarily be elected the "leader" thereafter.
  • Priority - The robot with the higher Priority is considered the leader. Priority is generated randomly at startup.
  • Mode - The robot implements a very simple finite state machine. It starts out waiting to bump into the other robot, then either is the initiator (Talking) or responder (Listening) in a conversation to elect a leader, after which it is either leading or following.

Immediately following the state type, we define the enum RobotMode this has the five possible modes that the robot can be in.

Now we'll define the message types. Recall that a message consists of a body and a response port. The body is always a request object; the response port is used to post a response object. Hence, we'll define here the message class itself (Converse), the request class (ConverseRequest), we do not need to define a response, so we will use the default response type for the verb, in this case DefaultSubmitResponseType. We call the message Converse because this message is used for both initiating and responding to conversation items, as if the robots were having a conversation. In addition, the request carries a Priority field, to keep the robots apprised of the each other's priorities. Please view the code below:

 public class Converse : Submit<ConverseRequest, DsspResponsePort<DefaultSubmitResponseType>>
{
}

[DataContract]
public class ConverseRequest
{
    private string _service;
    [DataMember]
    public string Service
    {
        get { return _service; }
        set { _service = value; }
    }

    ConverseMessage _message;
    [DataMember]
    public ConverseMessage Message
    {
        get { return _message; }
        set { _message = value; }
    }

    int _priority;
    [DataMember]
    public int Priority
    {
        get { return _priority; }
        set { _priority = value; }
    }
}

[DataContract]
public enum ConverseMessage
{
    // Hello, fellow robot.  Who will go first?
    HelloWhoWillGoFirst,
    // Let's find out.  Here is my random priority.
    LetsFindOutHereIsMyPriority,
    // Our priorities matched! Here's another try.
    PrioritiesMatchedHeresAnotherTry,
    // I'm going first!
    ImGoingFirst,
    // You go first.
    YouGoFirst,
    // I'm sorry, I didn't understand you.
    ImSorryIDontUnderstand,
    OverAndOut
}

The Converse class is empty, because it inherits all it needs from the Submit class, using the C# generics to say that the body is of type ConverseRequest and the response port is of type DsspResponsePort<DefaultSubmitResponseType>. Although the nested generics make for tricky syntax, they make code very reusable (as you can see here - creating your new mesage type required a single line of code). Next is the ConverseRequest, which defines a Service field (about which, more later), a Message field and a Priority field, as described. Recall that the DataContract and DataMember attributes indicate serializability. The Message field uses the enum type ConverseMessage which defines all possible messages. This is less prone to developer error than using a string, and more human readable than using an integer.

The other message type we define is used internally within the service. This is the SetMode message which the service sends to itself when it needs to change mode. This allows us to maintain consistency within the service.

 public class SetMode : Update<SetModeRequest, DsspResponsePort<DefaultUpdateResponseType>>
{
    public SetMode()
    {
    }

    public SetMode(RobotMode mode)
        : base(new SetModeRequest(mode))
    {
    }
}

[DataContract]
public class SetModeRequest
{
    public SetModeRequest()
    {
    }

    public SetModeRequest(RobotMode mode)
    {
        _mode = mode;
    }

    private RobotMode _mode;
    [DataMember]
    public RobotMode Mode
    {
        get { return _mode; }
        set { _mode = value; }
    }
}

The last thing we need to do is to add the new message type to the allowed types for the PortSet. Additionally, we'll also support the SetDrivePower message, which is defined as part of the generic drive service. All that is necessary is to add these types to the list of generics for the PortSet, as below:

 [ServicePort()]
public class Lab7Operations : PortSet<DsspDefaultLookup, DsspDefaultDrop, Get, drive.SetDrivePower, Converse, SetMode>
{
}

Again, the syntax here is somewhat complex, but the result is that a one-line code change supports your new message type. You can achieve a better understanding of the structure here by reading the DSS User Guide.

Step 3: Add Partnerships

Now we'll move on to define the behavior of our service. Open the file Lab7.cs . As before, add an alias to the differential drive namespace, but this time also add one to the bumper:

 using drive = Microsoft.Robotics.Services.Drive.Proxy;
using bumper = Microsoft.Robotics.Services.ContactSensor.Proxy;

Whereas we've used VPL to define all our partnerships for us before, now we'll do it by hand. To define a partnership, one must specify the Partner attribute as well as the operations port with which to send to and receive messages from the partner. Beneath the _mainPort declaration, add the Peer partner, as below. We use the creation policy UseExisting because we are contacting a remote node and do not want to create a peer instance on the local node. We use the flag Optional because we don't want the service to fail to load if our peer doesn't exist at startup.

 [Partner(
    "Peer",
    Contract = Contract.Identifier,
    CreationPolicy = PartnerCreationPolicy.UseExisting,
    Optional = true
)]
private Lab7Operations _peer = new Lab7Operations();

Next add the partnership for Drive. We use the policy UseExistingOrCreate so if there is not already a drive service running, the node will start one for us. We don't specify the Optional flag, but the default is false; we need the drive for this service to work properly.

 [Partner(
    "Drive",
    Contract = drive.Contract.Identifier,
    CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate
 )]
drive.DriveOperations _drivePort = new drive.DriveOperations();

The bumper partnership comes next. We have the same pattern as before, except we wish to subscribe to bumper notifications so we always receive them (a common pattern for interfacing with sensors). Hence, we also define the _bumperNotify port on which to receive subscriptions.

 [Partner(
    "Bumper",
    Contract = bumper.Contract.Identifier,
    CreationPolicy = PartnerCreationPolicy.UseExistingOrCreate
 )]
bumper.ContactSensorArrayOperations _bumperPort = new bumper.ContactSensorArrayOperations();
bumper.ContactSensorArrayOperations _bumperNotify = new bumper.ContactSensorArrayOperations();

We must also add one special partner: the InitialState partner. This partner handles loading the state from a config file, the location of which is specified in the manifest. Because we specify the InitialState Partner we have to be aware that the service state may be null at startup, this happens when a config file is not available. You will see in the code for the Start() method that we check for that condition.

 [InitialStatePartner(Optional = true)]
private Lab7State _state = new Lab7State();

Step 4: Bump into other Robot

To illustrate the need for coordination, we will start by having the robots bump into one another. Later we will introduce a method for them to determine who will go first and lead the other one.

We will now add initialization code to the service. In the Start method, which is called by the infrastructure for you automatically when the service starts, add the code below. First, we initialize the priority randomly (an integer between 0 and 9). Next, we start listening for notifications from the bumbers. Because we will want to know what the current mode of the service is when the bumper is pressed, this needs to be handled in a specific way to maintain the read-write lock on the state correctly. That is what the somewhat complicated code within the base.MainPortInterleave.CombineWith() method call is doing. For more information about the Interleave concept check the CCR User Guide.

Next we will start the iterator task DoAsynchronousStartup. CCR's concept of an iterator task is a powerful method of writing asynchronous, non-blocking code in a sequential manner. You can read more about it in the CCR User Guide. In that iterator, we subscribe to the bumper so we will actually receive notification whenever it is pressed (the syntax is that we send a Subscribe message on the regular _bumperPort, and the subscribe message tells the bumper service that we want to receive notifications on the _bumperNotify port). Then we wait for a short interval to allow the drive system to initialize (this is not neccessary on all hardware platforms). Finally we start driving forwards slowly.

 protected override void Start()
{
    if (_state == null)
    {
        _state = new Lab7State();
    }

    base.Start();

    _state.Priority = (new Random()).Next(0, 10);

    base.MainPortInterleave.CombineWith(
        new Interleave(
            new ExclusiveReceiverGroup(),
            new ConcurrentReceiverGroup(
                Arbiter.Receive<bumper.Update>(true, _bumperNotify, BumperUpdateHandler)
            )
        )
    );


    SpawnIterator(DoAsynchronousStartup);
}

IEnumerator<ITask> DoAsynchronousStartup()
{
    yield return Arbiter.Choice(
        _bumperPort.Subscribe(_bumperNotify),
        EmptyHandler,
        EmptyHandler
    );

    // wait 2 seconds to give the drive system time to intialize.
    yield return Arbiter.Receive(false, TimeoutPort(2000), EmptyHandler);

    // drive forward
    _drivePort.SetDrivePower(0.1, 0.1);
}

Now write the handler for the bumper notifications. When the robot is in the first TryingToBump state, and it bumps into something, we will assume that it is the other robot and move into the approriate next state, depending on whether or not we are the initiating party. If the robot is not in the TryingToBump state notifications will be processed but ignored.

 void BumperUpdateHandler(bumper.Update update)
{
    if (_state.Mode == RobotMode.TryingToBump &&
        update.Body.Pressed)
    {
        // set the new mode to either Talking or Listening as appropriate.
        if (_state.initiateDialog)
        {
            _mainPort.Post(new SetMode(RobotMode.Talking));
        }
        else
        {
            _mainPort.Post(new SetMode(RobotMode.Listening));
        }
        }
}

Currently, when the bumper is pressed nothing happens, this is because we have not implemented a handler for the SetMode message. The SetModeHandler method is decorated with the attribute ServiceHandler and, because it is derived from Update and modifies the service state, uses ServiceHandlerBehavior.Exclusive.

The RobotMode enum has been designed that the robot's mode only every increases. This allows us to avoid race conditions by simply ignoring requests where the new mode is not greater than the existing mode. The Listening and Talking modes start by backing the robot up a short distance and then the Talking mode initiates the conversation, the Listening mode just waits.

 [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public void SetModeHandler(SetMode update)
{
    RobotMode mode = update.Body.Mode;

    if (mode > _state.Mode)
    {
        _state.Mode = mode;

        switch (mode)
        {
            case RobotMode.TryingToBump:
                break;
            case RobotMode.Listening:
                SpawnIterator(false, BackUpAndStop);
                break;
            case RobotMode.Talking:
                SpawnIterator(true, BackUpAndStop);
                break;
            case RobotMode.Leading:
                SpawnIterator(StartLeading);
                break;
            case RobotMode.Following:
                break;
        }
    }
    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}

IEnumerator<ITask> BackUpAndStop(bool initiateConversation)
{
    // start driving backwards slowly
    _drivePort.SetDrivePower(-0.1, -0.1);

    // wait 2 seconds
    yield return Arbiter.Receive(false, TimeoutPort(2000), EmptyHandler);

    // stop
    _drivePort.SetDrivePower(0, 0);

    if (initiateConversation)
    {
        SendToPeer(ConverseMessage.HelloWhoWillGoFirst);
    }
}

At this point, implement a stub function for SendToPeer and StartListening, then test that your service has the expected functionality so far. It should drive forwards until it hits something, then back up a short distance and stop. Before you proceed, make sure that you understand how the service achieves this.

The last step is to begin communicating with the peer (if this robot is the initiator).

Step 5: Multirobot Communication and Decentralized Election

In this section, one of the robots is going to be elected the leader in a decentralized fashion via communicating random priorities. It is decentralized because both robots perform the computation of who wins independently, rather than a single robot determining the leader and then telling the other robots.

However, most of the code in this section handles the communication between the robots and defines a user-friendly protocol whereby the robots converse to set up the election.

Fill in your SendToPeer method from the last section with the code below. The purpose of this method is to send a message (the input argument) to the other robot. There are two overloads for this method, the first only takes the message as a parameter, the second takes a service address (as a string) in addition to the message. In the first overload the service address of the "Peer" partner is looked up using the FindPartner method, implemented on the standard service base class DsspServiceBase. The second overload, which actually sends the messages does three things

  1. Create a service forwarder - this allows us to send a message to a service given its URI, this allows communication with a service that we may not have a partnership with.
  2. Construct the message to send - the ConserveRequest data type has three properties, we fill the Service property with the service URI of our own service, this is in the ServiceInfo property of the service. This is how the responding service will know how to send a message to the initiating service.
  3. Send the message and play a tone - the beep serves no essential purpose, but does let you know that the robots are "talking" to each other.
 public void SendToPeer(ConverseMessage input)
{
    PartnerType partner = FindPartner("Peer");
    if (partner != null && !string.IsNullOrEmpty(partner.Service))
    {
        SendToPeer(partner.Service, input);
    }
    else
    {
        LogError(LogGroups.Console, "Unable to initiate converstation with peer. Partner is not established");
    }
}

public void SendToPeer(string service, ConverseMessage input)
{
    // make forwarder
    Lab7Operations fwdPort = ServiceForwarder<Lab7Operations>(service);

    // build the message
    Converse msg = new Converse();
    msg.Body.Service = ServiceInfo.Service;
    msg.Body.Message = input;
    msg.Body.Priority = _state.Priority;
    // we are not interested in a response.
    msg.ResponsePort = null;

    // send it
    System.Console.Beep(1600, 500);
    _peer.Post(msg);

    LogInfo(LogGroups.Console, "  Sent: " + input + " to: " + service);
}

The SendToPeer function is complete, but sending the Converse message is not useful if the service you are sending it to does not handle it. So now implement the ConverseHandler method as shown below. This too does three steps:

  1. Beeps a little - for the same reasons as above
  2. If the message is not the terminating OverAndOut, computes a response and sends it to the peer service by calling the SendToPeer - try and understand the advantages that this symmetric approach has.
  3. Posts a response to the message response port - This is essential. All message handlers MUST post responses, otherwise services that are waiting for a response will deadlock, unless they set a timeout.

To compute the correct response a simple state machine is implemented that decides what the correct response is based on the message and relative priority values, that is what we will be implementing next...

 [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public void ConverseHandler(Converse converse)
{
    ConverseRequest request = converse.Body;

    LogInfo(LogGroups.Console, "    Rcvd: " + request.Message);
    System.Console.Beep(2400, 250);
    System.Console.Beep(1200, 250);

    if (request.Message != ConverseMessage.OverAndOut)
    {
        // Send conversation response.
        ConverseMessage response = GetNextStep(request.Message, request.Priority);
        SendToPeer(request.Service, response);
    }

    // Send DSSP response.
    converse.ResponsePort.Post(DefaultSubmitResponseType.Instance);
}

The GetNextStep method implements the communications protocol, as below. It is a switch statement, responding to a particular message with a particular response.

 private ConverseMessage GetNextStep(ConverseMessage input, int priority)
{
    ConverseMessage output = ConverseMessage.OverAndOut;

    switch (input)
    {
        case ConverseMessage.HelloWhoWillGoFirst:
            output = ConverseMessage.LetsFindOutHereIsMyPriority;
            break;

        case ConverseMessage.LetsFindOutHereIsMyPriority:
        case ConverseMessage.PrioritiesMatchedHeresAnotherTry:
            LogInfo(LogGroups.Console, "Comparing priorities...");
            LogInfo(LogGroups.Console, "I have: " + _state.Priority + ", peer has: " + priority);
            if (priority == _state.Priority)
            {
                _state.Priority = (new Random()).Next(0, 10);
                output = ConverseMessage.PrioritiesMatchedHeresAnotherTry;
            }
            else if (priority < _state.Priority)
            {
                _mainPort.Post(new SetMode(RobotMode.Leading));
                output = ConverseMessage.ImGoingFirst;
            }
            else
            {
                _mainPort.Post(new SetMode(RobotMode.Following));
                output = ConverseMessage.YouGoFirst;
            }
            break;

        case ConverseMessage.ImGoingFirst:
            _mainPort.Post(new SetMode(RobotMode.Following));
            break; // default response is OverAndOut - used for end of conversation

        case ConverseMessage.YouGoFirst:
            _mainPort.Post(new SetMode(RobotMode.Leading));
            break; // default response is OverAndOut - used for end of conversation

        case ConverseMessage.OverAndOut:
        case ConverseMessage.ImSorryIDontUnderstand:
            break; // default response is OverAndOut - used for end of conversation

        default:
            output = ConverseMessage.ImSorryIDontUnderstand;
            break;
    }

    return output;
}

The only notable pieces of this code are:

  • If the priorities match, we regenerate our priorities and resend them.
  • The case Let's find out. Here is my random priority falls through to Our priorities matched! Here's another try, so the behavior for both of these statements is the same.
  • We set IsLeader if our priority is higher or if the peer defers leadership to us.
  • When we determine that we are the leader, we send a SetMode message to ourselves with the new mode Leading; otherwise, if we determine we are not the leader, we send a SetMode message with the new mode Following.

And now we have defined the protocol. Since it is difficult to read the switch statement, a typical transcript of a dialog is below:

  • Robot 1: HelloWhoWillGoFirst - "Hello, fellow robot. Who will go first?"
  • Robot 2: LetsFindOutHereIsMyPriority - "Let's find out. Here is my random priority."
  • Robot 1: YouGoFirst - "You go first."

Step 6: Follow the Leader

After the leader has been elected, the lead robot will turn around (so that both robots are facing the same direction - we assume they start out facing one another), and then start performing motions and sending those motions to the follower after a short delay. The result will be "follow the leader" behavior.

 public virtual IEnumerator<ITask> StartLeading()
{

    LogInfo(LogGroups.Console, "starting to lead motion...");

    // first, stop!
    _drivePort.SetDrivePower(0, 0);

    // turn around, so peer is behind you but oriented like you are
    _drivePort.SetDrivePower(.3, -.3);
    yield return Arbiter.Receive(
        false,
        TimeoutPort(3000),
        delegate(DateTime t) { }
    );
    _drivePort.SetDrivePower(0, 0);

    while (true)
    {

        // wait a bit
        yield return Arbiter.Receive(
            false,
            TimeoutPort(1000),
            delegate(DateTime t) { }
        );

        // drive along an arc
        _mainPort.Post(new drive.SetDrivePower(new drive.SetDrivePowerRequest(.3, .6)));

        // wait a bit
        yield return Arbiter.Receive(
            false,
            TimeoutPort(1000),
            delegate(DateTime t) { }
        );

        // drive along opposite arc
        _mainPort.Post(new drive.SetDrivePower(new drive.SetDrivePowerRequest(.6, .3)));

    }

}

Add the StartLeading method, which performs the rotation and then loops to drive along an arc for a brief period and then drive along the opposite arc to produce a rough sinusoidal motion. Notice that we post the SetDrivePower message to the _mainPort, which is in fact the service's own port. While it might seem strange that a service post a message to itself, in this case it abstracts away where the message came from and allows both the follower and the leader to handle the message in the same way (see the DoMotion handler below).

 [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public void DoMotion(drive.SetDrivePower message)
{
    // do it
    _drivePort.Post(message);

    if (_state.Mode == RobotMode.Leading)
    {
        // wait one second
        Activate(
            Arbiter.Receive(false, TimeoutPort(1000),
                delegate(DateTime t) 
                {
                    // tell the follower to do it
                    _peer.Post(message);
                }
            )
         );
    }

}

DoMotion simply forwards the SetDrivePower message to the differential drive, and, if the robot is the leader, it will also send the message to its peer after a short delay.

Since the leader is determining the motion to be executed, this multirobot coordination paradigm is an example of centralized control. Now you've seen both centralized and decentralized methods.

Try It Out

To run the completed code in the Simulation Environment, run Lab7.manifest.xml.

To run your completed code on physical robots, you'll need to specify the robot hostnames in the manifests so each robot knows how to contact the other. Included are the manifests Introvert.manifest.xml and Extrovert.manifest.xml. Edit these manifests with the Manifest Editor to make sure the peer service URL points to the appropriate remote machine:

Figure 1

Figure 1 - Remote Service URI

For your program to work, the correct hardware services (drive and bumper) need to be running. One way to achieve this is to add the services to the extrovert and introvert manifests. You can then run the manifests using dsshost.exe. Alternatively you can use dsshost.exe to start each node and then add the appropriate hardware services from from the control panel. Once these services are started, you can then start the introvert and extrovert manifests, also from the control panel (under Lab7). If you are using the Create, then to start the hardware service from the control panel, just look up iRobot Generic Contact Sensors and select the manifest specified (this will start both the drive and bumper services you require).

Note, no matter which method you use, make sure to run the introvert first, since the extrovert will fail on startup if it is not able to find an existing introvert [again, cross-node dynamic partnering is outside the scope of this tutorial]).

Summary

In this tutorial, you learned how to:

  • Create Service
  • Define Types
  • Add Partnerships
  • Bump into other Robot
  • Multirobot Communication and Decentralized Election
  • Follow the Leader

 

 

© 2008 Microsoft Corporation. All Rights Reserved.