Robotics Tutorial 5 (C#) - Using Advanced Services

Glossary Item Box

Microsoft Robotics Developer Studio

Send feedback on this topic

Robotics Tutorial 5 (C#) - Using Advanced Services

Microsoft Robotics Developer Studio (RDS)can implement the behavior of a robot by writing Orchestration services. Orchestration services communicate with services that perform specific functions and can represent the interface to software or hardware of the robot.

This tutorial describes how to build an Orchestration service that realizes an Explorer behavior for a mobile robot equipped with a laser-based distance measuring device.

This tutorial consists of this document only and there is no project file associated to it.

This tutorial teaches you how to:

• Define Behavior.
• Select Services to Orchestrate.
• Implement the Behavior.
• Fine-Tune the Behavior.
• Skip Old Notifications.
• Use a Watchdog Timer to Monitor a Service.

See Also:

• Getting Started

Prerequisites

Hardware

This tutorial is designed for use with a mobile robot that has a two wheel differential/skid drive, front and rear contact sensor, and a forward facing 180 degree SICK Laser Range Finder device.

The service that represents the drive must implement the two-wheel differential drive contract, https://schemas.microsoft.com/robotics/2006/05/drive.html

The service that represents the contact sensors must implement the contact sensor array contract, https://schemas.microsoft.com/2006/06/contactsensor.html

Both service contracts were introduced in Robotics Tutorial 2 (C#) - Coordinating Services. The RDS samples contain an implementation of these service contracts for the MobileRobots Pioneer 3DX platform in the MobileRobots folder.

The RDS samples also contain a service for the SICK Laser Range Finder in the SickLRF folder.

If these platforms are not available to you, you can also build your own robot in the simulator.

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.

Getting Started

The service for this tutorial already exists and is available under the setup install directory in the samples\Misc\Explorer folder.

Begin by loading Explorer.csproj in Visual Studio.

After you have built the project, you can run it from the DSS Command Prompt:

dsshost /port:50000 /manifest:"samples\config\Explorer.manifest.xml"

This manifest loads services specific to Pioneer 3DX. If you want to use a different platform, you must change the manifest. Alternatively, you can start services manually from the Control Panel.

You can also setup Microsoft Visual Studio to start the sample from within Visual Studio:

1. In the Solution Explorer double click on the projects Properties folder.
2. In the Debug tab select "Start external program" and specify dsshost.exe
   You can find this file in the bin subdirectory of the setup install directory. For example, C:\Microsoft RDS\bin\dsshost.exe.
3. Use /port:50000 /manifest:"config\Explorer.manifest.xml" as command line option and the setup installation folder as working directory.

You can now run the sample by pressing F5 or choosing the Debug > Start Debugging menu command.

Step 1: Define Behavior

The objective of this tutorial is to describe how an "Explorer" service for a mobile robot is built. Note that the service is already created and the code is in Samples\Misc\Explorer. To build this service, you need to refine what "exploring" means and define desired behavior of the robot. What you want the robot to do is the following:

1. Whenever there is a sufficiently large open space ahead, move into the space. Of course, this is not sufficient to be called explorer because the robot might easily get stuck in a cul-de-sac. Hence, you also need:

   1. If there is no space ahead, scan the entire environment and select the direction with the largest open space to move into. These rules are seemingly sufficient for the robot to roam around office corridors, but our fellow colleagues from across the corridor also insist on:

   2. Avoid running into obstacles, and...

   3. If an obstacle is accidentally hit, stop and retreat.

Be aware that obstacles may turn up suddenly. For example, when the colleagues inhabiting the offices across the corridor jump in front of your creation. Robots seem to have this effect on people. So you want to make sure that rules 3 and 4 have precedence over 1 and 2.

Step 2: Select Services to Orchestrate

In order to implement this behavior, it helps to understand the capabilities of your hardware and the services that control it. This tutorial is based on the assumption that you have a MobileRobots Pioneer 3-DX platform with a SICK Laser Range Finder mounted on top or something very similar. But you can also construct a similar robot, physically or by using the simulator.

Figure 1 - Sensors in Explorer robot

The Pioneer 3 has a differential drive configuration. This allows you to turn the robot in place. The Robotics Developer Studio samples contain a service called ArcosDrive that implements the abstract Drive Contract (see Robotics Tutorial 2 (C#) - Coordinating Services). The Drive Contract has operations to: set the left and right wheel velocities individually, rotate the entire drive by a specified angle, and to drive a specified distance. The drive service sends notification when its state changes, including the wheel velocity and whether the drive is enabled.

You can configure a Pioneer 3-DX with a front and rear bumper ring consisting of multiple contact sensors. The Robotics Developer Studio samples contain a service called ArcosBumper that implements the abstract contact sensor contract (again, see Robotics Tutorial 2 (C#) - Coordinating Services). The service sends a notification when a bumper is pressed or released. You can use the bumpers to detect the unfortunate event that the robot ran into an obstacle.

The SICK Laser Range Finder is a distance measuring device. A laser range finder (LRF) measures the distance to an obstacle by sending a laser beam and measuring the time until its reflection is received. Most laser range finders use a rotating mirror to scan the segment of an arc instead of a single point. The accuracy of the LRF is determined by its spatial resolution (e.g. number of distance readings per scan) and temporal resolution (scan frequency). This tutorial assumes the following configuration:

• 180° scan segment
• 181 distance readings per segment (1 degree resolution)
• 5 scans per second

The Robotics Developer Studio samples contain a service called SickLRF that represents a SICK Laser Range Finder Device. The service sends a notification for every scan. A scan is a set of readings where each reading contains the measured distance at a specific angle.

Figure 2 - SICK Laser Range Finder

Using these services, you can now define the orchestration to be implemented by the Explorer service. The explorer service has to subscribe to the bumper, drive, and LRF service. Whenever it receives a notification from any of these services, the Explorer service must send a command to the drive service. For example: If the bumper service sends a notification that indicates that a bumper is pressed, the Explorer service sends a command to the drive service to stop and reverse.

Figure 3 - Explorer Service Process Flow

Create a skeleton for the Explorer service by using dssnewservice. In order to use these services, you have to add references to the proxy assemblies RoboticsCommon.Proxy.dll and SickLRF.Y2005.M12.Proxy.dll. Both are distributed with Robotics Developer Studio.

You can now add partner declarations for the drive, bumper, and laser range finder to your Explorer service class. In the Start() method of explorer, subscribe to all partners and activate handler methods for the notifications.

Step 3: Implement the Behavior

In Step 1, you defined the desired behavior. The implementation of this behavior has to be driven by the different notifications outlined in Step 2. Be aware that any of these notifications can arrive at any time independent of which state the explorer is in. For example, LRF notifications arrive periodically but the resulting action clearly depends on whether the explorer is going forward or scanning the environment for a new direction.

The classic approach to implement such an event-driven, state-based behavior is using a state machine.

For the behavior outlined in Step 1 it is easy to identify three states:

• Moving: The robot has free space ahead and drives forward.
• Mapping: The robot turns to scan its environment for a new direction.
• Unknown: A close obstacle was detected ahead or the robot was stopped after a collision. This is also the initial state.

The states Moving and Mapping are grouped into the Active meta-state.

Figure 4 - Behavior States

By adding transitions between these states, you can implement the Explorer behavior. Transitions are triggered by the notifications received from partner services.

Figure 5 - Transitions between states

Because the laser range finder is the only means of scanning the immediate environment, it is not surprising that most transitions are triggered by notifications from the laser range finder. The only exception is an emergency stop caused by a pressed bumper.

The states Moving and Mapping are meta-states themselves:

Figure 6 - Transitions between states

Mapping requires a 360° scan. Because the LRF can only scan 180°, mapping is performed in multiple stages. When open space is found during mapping, the robot has to adjust its heading in order to enter this space.

Now that the behavior is defined in terms of a state machine, you can implement it on your Explorer service. First, you need to keep track of the state the explorer is in. Add an enumeration definition representing the different states to your types:

[DataContract]
public enum LogicalState
{
    Unknown,
    AdjustHeading,
    FreeForwards,
    MapSurroundings,
    MapNorth,
    MapSouth
}

[DataContract]
public class State
{
    private LogicalState _logicalState;

    [DataMember]
    public LogicalState LogicalState
    {
        get { return _logicalState; }
        set { _logicalState = value; }
    }
}

Add properties to get the correct meta-state for the LogicalState:

[DataContract]
public class State
{
    ...
    internal bool IsMapping
    {
        get
        {
            return
                LogicalState == LogicalState.MapNorth ||
                LogicalState == LogicalState.MapSouth ||
                LogicalState == LogicalState.MapSurroundings;
            }
        }
    }

    internal bool IsUnknown
    {
        get
        {
            return LogicalState == LogicalState.Unknown;
        }
    }

    internal bool IsMoving
    {
        get
        {
            return IsActive && !IsMapping;
        }
    }

    internal bool IsActive
    {
        get
        {
            return !IsUnknown;
        }
    }
    ...
}

In the handlers for notifications from the laser range finder or bumper, you can access and change the state according to the state machine. Note that every notification might change the state, meaning your handler must be executed inside the ExclusiveReceiverGroup of your services activation! Because you update your state with every notification, it is good practice to do this by posting an update message to Explorer service itself.

To update the state based on laser data, add a LaserRangeFinderUpdate request to your types and to the operations port:

class ExplorerOperations : PortSet<
    DsspDefaultLookup,
    DsspDefaultDrop,
    Get,
    LaserRangeFinderUpdate
    >
{}

class LaserRangeFinderUpdate : Update<
    sicklrf.State,
    PortSet<defaultupdateresponsetype>
    >
{
    public LaserRangeFinderUpdate(sicklrf.State body)
        : base(body)
    { }

    public LaserRangeFinderUpdate()
    { }
}

In the handler for LRF notifications, you can now post this request to the Explorer's own main port:

class Explorer : DsspServiceBase
{
    ...
    IEnumerator<itask> LaserReplaceNotification(sicklrf.Replace replace)
    {
        LaserRangeFinderUpdate request =
            new LaserRangeFinderUpdate(replace.Body);

        _mainPort.Post(request);
    }
    ...
}

You should handle other notifications, from the bumper and drive services, in a similar fashion.

Now implement the laser-triggered transitions in the request handler for the LaserRangeFinderUpdate request:

class Explorer : DsspServiceBase
{
    ...
    void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate upate)
    {
        sicklrf.State laserData = upate.Body;
        int distance = FindNearestObstacleInCorridor(
          laserData, CorridorWidthMapping, 45);

        AvoidCollision(distance);
        UpdateLogicalState(laserData, distance);
        upate.ResponsePort.Post(DefaultUpdateResponseType.Instance);
    }
    ...
}

The method FindNearestObstacleInCorridor examines the data received from the laser range finder and returns the distance of the closest obstacle found in corridor of the specified width by restricting the field of view to 45° to the left and right.

If the distance is below a certain threshold, the Explorer transitions to the Unknown state and stops. Of course this makes only sense when it is in the Moving state:

class Explorer : DsspServiceBase
{
    ...
    private void AvoidCollision(int distance)
    {
        if (distance > ObstacleDistance && _state.IsMoving)
        {
            StopMoving();
            _state.LogicalState = LogicalState.Unknown;
        }
    }
    ...
}

After making sure that the robot doesn't run into anyone, you can now select the most appropriate transition based on the robot's current state. The following example illustrates this for the Adjust Heading state. The NewHeading property of the Explorer's State stores the angle at which to find the widest open space that was discovered in the Mapping state. TheAdjustHeading() method sends a RotateDegrees request to the drive and transitions to the Free Forward state. The behavior of the other states is implemented in a similar way, please see Explorer.cs for details.

private void UpdateLogicalState(sicklrf.State laserData, int distance)
{
    if (_state.IsUnknown)
    {
        StartMapping(laserData, distance);
    }
    else if (_state.IsMoving)
    {
        Move(laserData, distance);
    }
    else if (_state.IsMapping)
    {
        Map(laserData, distance);
    }
}

private void Move(sicklrf.State laserData, int distance)
{
    switch (_state.LogicalState)
    {
        case LogicalState.AjustHeading:
            AdjustHeading();
            break;
        case LogicalState.FreeForwards:
            AdjustVelocity(laserData, distance);
            break;
        default:
            LogInfo("Explorer.Move() called in illegal state");
            break;
        }
    }
}

private void AdjustHeading()
{
    LogInfo("Step Turning to: " + _state.NewHeading);
    Turn(_state.NewHeading); // send RotateDegrees request

    _state.LogicalState = LogicalState.FreeForwards;
}

Notifications from the bumper service are handled in the same way:

void BumperUpdateHandler(BumperUpdate update)
{
if (_state.IsActive && update.Body.Pressed)
    {
        Bumped();
    }
    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}

private void Bumped()
{
    if (_state.Velocity <= 0.0)
    {
        LogInfo("Rear and/or Front bumper pressed, Stopping");
        // either a rear bumper or both front and rear
        // bumpers are pressed. STOP!
        StopTurning();
        StopMoving();

        _state.LogicalState = LogicalState.Unknown;
    }
    else
    {
        // only a front bumper is pressed.
        // move back <backupdistance> mm;
        StopTurning();
        Translate(BackupDistance);

        _state.LogicalState = LogicalState.Unknown;
    }
}

Step 4: Fine-Tune the Behavior

If you run the Explorer as described so far, its behavior would appear jumpy and nervous and sometimes wouldn't even do what you expect. The reason for this is simple: every laser notification causes the Explorer to make a new decision concerning its state, even if the previous decision is still good and valid.

In some cases this is only annoying, in others it's just wrong. For example, any new notification would trigger the transition from Mapping South to Mapping North, even if Mapping South is not finished (180° rotation not completed). There are many solutions to this problem. For example, you could delay the transition to Mapping North until both wheels have come to a full stop, using the wheel velocity information from the drive service.

A much simpler solution is the use of a counter that decreases with every notification. The notification is handled only once the counter reaches zero:

private void UpdateLogicalState(sicklrf.State laserData, int distance)
{
    if (_state.Countdown > 0)
    {
        _state.Countdown--;
    }
    else if (_state.IsUnknown)
    {
        StartMapping(laserData, distance);
    }
    else if (_state.IsMoving)
    {
        Move(laserData, distance);
    }
    else if (_state.IsMapping)
    {
        Map(laserData, distance);
    }
}

After changing the LogicalState, you have to set the counter to the number of laser notifications that can safely be ignored. You should do this whenever you change the LogicalState.

private void AdjustHeading()
{
    LogInfo("Step Turning to: " + _state.NewHeading);
    Turn(_state.NewHeading); // send RotateDegrees request

    _state.LogicalState = LogicalState.FreeForwards;
    _state.Countdown = Math.Abs(_state.NewHeading / 10);
}

However, the counter must not be applied to transitions that avoid or handle collisions.

The Explorer can be tweaked in many more ways. You will find some tweaks in the Explorer sample. For example, the robot adjusts its velocity based on the available open space. It decelerates when it approaches an obstacle and accelerates as more space opens up instead of just switching between full stop and maximum velocity.

Furthermore, the Explorer class defines a set of constants such as the safety distance within which an obstacle is considered a danger or the robots maximum velocity. These values depend on the configuration of the robot. If your robot can travel at 20 km/h and needs 10 m to come to a standstill, you might consider increasing the ObstacleDistance from its current setting (0.5 m). You may also decide to expose those values on the Explorer's state type and make them configurable by using an initial state partner.

Step 5: Skip Old Notifications

Many services send notifications periodically. Notifications queue up in the port until they are handled. This means that potentially outdated information is unnecessarily kept around and eventually handled. If the message handling is computationally expensive or the messages are large, this is undesirable. Even worse, notifications may arrive faster than you are able to handle them and they will pile up until you run out of memory.

Thus, in some situations it is necessary to only handle the most recent notification and skip those received before. For the Explorer, the notifications from the LRF are a good example. It makes no sense to analyze old data when new data is available.

Ports have a Test() method that returns the next message in the port or null if the port is empty. You can use this method to read all messages from the port and handle the most recent one. However, Test() this only works on ports that have no persisted receiver activated. This means that after handling a message, you have to reactivate a handler on this port to be able to receive the next message.

IEnumerator<ITask> LaserReplaceNotification(sicklrf.Replace replace)
{
    sicklrf.State laserData =
        GetMostRecentLaserNotification(replace.Body);

    LaserRangeFinderUpdate request = new
        LaserRangeFinderUpdate(laserData);

    _mainPort.Post(request);

    yield return Arbiter.Choice(
        request.ResponsePort,
        delegate(DefaultUpdateResponseType response) { },
        delegate(Fault fault) { }
    );

    GetMostRecentLaserNotification(laserData);

    // Reactivate the handler.
    Activate(
        Arbiter.ReceiveWithIterator<sicklrf.Replace>(false, _laserNotify, LaserReplaceNotification)
    );
}

private sicklrf.State GetMostRecentLaserNotification(sicklrf.State laserData)
{
    sicklrf.Replace testReplace;

    // _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
    // the portset contains
    int count = _laserNotify.P3.ItemCount - 1;

    for (int i = 0; i < count; i++)
    {
        testReplace = _laserNotify.Test<sicklrf.Replace>();
        if (testReplace.Body.TimeStamp > laserData.TimeStamp)
        {
            laserData = testReplace.Body;
        }
    }

    if (count > 0)
    {
        LogInfo(string.Format("Dropped {0} laser readings (laser start)", count));
    }
    return laserData;
}

You must also change the activation of the notification handler in the Start() method.

In the Start() method delete the following arbiter from the Interleave.

Arbiter.ReceiveWithIterator<sicklrf.Replace>(false, _laserNotify, LaserReplaceNotification)

And add a separate activation for LaserReplaceNotification.

protected override void Start()
{
    ...

    #region notification handler setup
    Activate(
        Arbiter.Interleave(
            new TeardownReceiverGroup(),
            new ExclusiveReceiverGroup(),
            new ConcurrentReceiverGroup(
                Arbiter.Receive<sicklrf.Reset>(true, _laserNotify, LaserResetNotification),
                Arbiter.Receive<drive.Update>(true, _driveNotify, DriveUpdateNotification),
                Arbiter.Receive<bumper.Replace>(true, _bumperNotify, BumperReplaceNotification),
                Arbiter.Receive<bumper.Update>(true, _bumperNotify, BumperUpdateNotification)
            )
        )
    );

    // We cannot replicate the activation of laser notifications because the
    // handler uses Test() to skip old laser notifications.
    Activate(
        Arbiter.ReceiveWithIterator<sicklrf.Replace>(false, _laserNotify, LaserReplaceNotification)
    );
    #endregion

    ...
}

Step 6: Use a Watchdog Timer to Monitor a Service

Any part of a robot, software or hardware, can fail. You should be aware of this when writing your application. If the failure of a component impacts your specific application, you should handle it.

For example the laser range finder may stop delivering data for various reasons. This is a potentially dangerous situation because the robot would be unable to detect obstacles. In this case it is advisable to stop the robot and wait until the laser range finder continues sending data. Thus, you add a new transition to your state machine.

Figure 7 - Monitor the service for signals

The question is, "When should I trigger this transition?" While all other transitions are notification triggered, this transition must be triggered by the absence of notifications. One way to check for absent notifications is to store the time at which the last notification was received and regularly check this timestamp. If the timestamp is too old, it is best assume that the laser range finder failed.

Add a new WatchDogUpdate request containing a DateTime time stamp as body to your types and the operations port and activate a handler for this request in the services main activation. This handler may change the state so it must be in the ExclusiveReceiverGroup. In the Start() method, post a new WatchDogUpdate request to start the timer.

protected override void Start()
{
    ...

    // Start watchdog timer
    _mainPort.Post(new WatchDogUpdate(
        new WatchDogUpdateRequest(DateTime.Now)
    ));

    // Create Subscriptions
    _bumperPort.Subscribe(_bumperNotify);
    _drivePort.Subscribe(_driveNotify);
    _laserPort.Subscribe(_laserNotify);

    DirectoryInsert();
}

Store the time when the latest laser notification was received in the Explorer's state:

void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate upate)
{
    sicklrf.State laserData = upate.Body;
    _state.MostRecentLaser = laserData.TimeStamp;

    ...
}

In the WatchDogUpdate request handler, check the time stamp and, if necessary, stop the robot.

void WatchDogUpdateHandler(WatchDogUpdate update)
{
    TimeSpan sinceLaser = update.Body.TimeStamp - _state.MostRecentLaser;
    if (sinceLaser.TotalMilliseconds >= WatchdogTimeout &&
        !_state.IsUnknown)
    {
        LogInfo("Stop requested, last laser data seen at " +
          _state.MostRecentLaser);
        StopMoving();
        _state.LogicalState = LogicalState.Unknown;
    }

    Activate(
        Arbiter.Receive(
            false,
            TimeoutPort(WatchdogInterval),
            delegate(DateTime ts)
            {
                _mainPort.Post(new WatchDogUpdate(
                    new WatchDogUpdateRequest(ts)
                ));
            }
        )
    );

    update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}

The watch dog handler reactivates an anonymous delegate on the timeout port every time it is called. When the timeout fires it posts a message to the main port, which causes it to start over.

Summary

In this tutorial, you learned how to:

• Define Behavior.
• Select Services to Orchestrate.
• Implement the Behavior.
• Fine-Tune the Behavior.
• Skip Old Notifications.
• Use a Watchdog Timer to Monitor a Service.

 

 

© 2012 Microsoft Corporation. All Rights Reserved.