The first thing to add to the Wander service is a reference to the NXT Sonar namespace. You should add the following code below the other references in the Wander service:

using sonar = Microsoft.Robotics.Services.Sample.Lego.Nxt.SonarSensor.Proxy;

The next thing to add is a partner attribute, which represents the NXT sonar service. This is used to subscribe to the NXT’s sonar sensor, and the code should be added below the other partner declarations.

[Partner("sonar", Contract = sonar.Contract.Identifier,
       CreationPolicy = PartnerCreationPolicy.UseExisting)]
private sonar.UltrasonicSensorOperations _sonarPort = new
       sonar.UltrasonicSensorOperations();
private sonar.UltrasonicSensorOperations _sonarNotifyPort = new
       sonar.UltrasonicSensorOperations();

You need to add a line to the ConcurrentReceiverGroup, which registers the handler for the sonar sensor. Add the following line of code to the Interleave pattern located in the Start method:

Arbiter.Receive<sonar.SonarSensorUpdate>(true, _sonarNotifyPort, SonarHandler),

Additionally, you need to subscribe to the sonar notification port, which in this case is named _sonarNotifyPort. This is accomplished by adding the following code to the Start method:

_sonarPort.Subscribe(_sonarNotifyPort);

You now need to create the handler that is invoked each time a notification arrives on the sonar notification port. The following code can be used for the SonarHandler, which can be added below the other message handlers:

private void SonarHandler(sonar.SonarSensorUpdate notification)
{
    // Call a function to get the most recent notification since
    // we may have several of them queued up before this handler
    // was called
    sonar.SonarSensorState sonarData =
      GetMostRecentSonarNotification(notification.Body);

    // check to see that we have some valid data to process
    if (sonarData.Distance != 0)
    {
        if (sonarData.Distance < 30)
       {
          SpawnIterator(ReverseAndTurn);
          LogInfo("the sonar detected an object too close to the robot");
         _state.MostRecentNotification = sonarData.TimeStamp;
       }
    }
}
private sonar.SonarSensorState GetMostRecentSonarNotification(sonar.SonarSensorState sonarData)
{ Return sonarData; }

Note

This code block contains method stubs that represent the GetMostRecentSonarNotification function. The body for this function will be covered later in the section.

The SonarHandler makes a call to the GetMostRecentSonarNotification function, which passes back the message for the most recent notification. It is possible for the sonar sensor to record multiple readings before the sonar handler has been called. In these cases, we deal only with the last notification. The code for the GetMostRecentSonarNotification is as follows (you need to add the code only to the method stub that we created in an earlier step):

private sonar.SonarSensorState GetMostRecentSonarNotification(sonar.SonarSensorState sonarData)
{
     sonar.SonarSensorUpdate temp;

     // Get the number of messages queued up on the sonar notification port
     int count = _sonarNotifyPort.Ports.Count - 1;

     // Loop through all the messages queued up on this port
     // since we need to find the last one
     for (int i = 0; i < count; i++)
     {
         // The test method will remove a message from the port
         temp = _sonarNotifyPort.Test<sonar.SonarSensorUpdate>();

         // Return the last one by comparing the timestamps
         if (temp.Body.TimeStamp > sonarData.TimeStamp)
         {
              sonarData = temp.Body;
         }
     }

     // Send back the last notification message
     return sonarData;
}

The Distance property contains the distance in centimeters to the nearest detectable object. If this value is below a certain threshold value (that you define), then an object is too near and a collision is imminent. Therefore, you want to command the robot to reverse and turn away from the object.

In version 1, the only way you could stop the robot was to push a button on the LEGO brick. Additionally, we can use the sound sensor to provide an alternative shutdown mechanism. Both the sound and light sensors on the LEGO NXT are known as analog sensors. You could use the generic analog sensors service to access this sensor, but for the LEGO NXT, a LEGO NXT Sound Sensor is specifically provided. Use the following code to add a reference to this namespace:

using sound = Microsoft.Robotics.Services.Sample.Lego.Nxt.SoundSensor.Proxy;

You will also need to add a partner declaration for the LEGO NXT Sound Sensor service. The following code can be placed below the other partner declarations:

// Add partner for the NXT sound
[Partner("NxtSound", Contract = sound.Contract.Identifier,
     CreationPolicy = PartnerCreationPolicy.UseExisting)]
private sound.SoundSensorOperations _soundPort = new
     sound.SoundSensorOperations();
private sound.SoundSensorOperations _soundNotifyPort = new
     sound.SoundSensorOperations();

As with the sonar sensor, you will need to add the following code to the Start method, which registers the handler named SoundHandler.

Arbiter.Receive<sound.SoundSensorUpdate>(true, _soundNotifyPort, SoundHandler)

Additionally, you need to add the following code to the Start method to subscribe to the service and receive notifications from the sound notify port:

_soundPort.Subscribe(_soundNotifyPort);

The next thing to add is the following code for the handler named SoundHandler:

private void SoundHandler(sound.SoundSensorUpdate notification)
{

     // check to see that the sound is above
     // a 35% sensor reading level
     if (notification.Body.Intensity > 35)
     {
         // Stop the robot
         SpawnIterator<double, double>(0, 0, DriveRobot);
         LogInfo("the sound sensor detected a noise; robot will now stop");
         state.MostRecentNotification = notification.Body.TimeStamp;
      }

}

If the Intensity property contains a value greater than 35, it commands the robot to stop.The LEGO NXT sound sensor detects decibels and returns the data in the form of a percentage. The higher the percentage, the louder the sound. In the SoundHandler for version 2, we are using a threshold value of 0.35, or 35 percent. You may have to experiment with your LEGO NXT to find the threshold level that works properly for your environment.

Because it is possible for the wheels of the LEGO NXT to get caught on a surface or for sensors to stop functioning correctly, it is a good idea to have a watchdog timer added to your roaming service. The watchdog timer acts as a safety guard that stops the robot if no sensor activity is reported over a period of time. To add a watchdog timer, we need to add a state variable that holds the data and time the last sensor notification was received. To create this state variable, you should add the following code to the WanderTypes class file:

private DateTime _mostRecentNotification;

[DataMember]
public DateTime MostRecentNotification
{
    get { return _mostRecentNotification; }
    set { _mostRecentNotification = value; }
}

You also need to add an operation named WatchDogUpdate to the PortSet for the WanderOperations, along with the following code:

public class WatchDogUpdate : Update<WatchDogUpdateRequest,
PortSet<DefaultUpdateResponseType, Fault>>
{
    public WatchDogUpdate(WatchDogUpdateRequest body) : base(body) { }

    public WatchDogUpdate() { }
}

[DataContract]
public class WatchDogUpdateRequest
{
    private DateTime _timeStamp;

    [DataMember]
    public DateTime TimeStamp
    {
        get { return _timeStamp; }
        set { _timeStamp = value; }
    }

    public WatchDogUpdateRequest(DateTime timeStamp)
    {
        TimeStamp = timeStamp;
    }

    public WatchDogUpdateRequest()
    { }
}

Because the watchdog timer will be updating the service state, you need to add the handler to the exclusive receiver group. Add the following code to the Start method in the Wander class file:

Arbiter.Receive<WatchDogUpdate>(true, _mainPort, WatchDogUpdateHandler)

After adding the preceding statement, the section of code that declares all the handlers should look like the following:

Activate(
     Arbiter.Interleave(
       new TeardownReceiverGroup(),
       new ExclusiveReceiverGroup(
          Arbiter.Receive<WatchDogUpdate>(true, _mainPort,
             WatchDogUpdateHandler)),
       new ConcurrentReceiverGroup(
          Arbiter.Receive<bumper.TouchSensorUpdate>(true,
           _nxtTouchSensorNotify, TouchSensorHandler),
          Arbiter.Receive<lego.ButtonsUpdate>(true,
           _legoNotifyPort, LegoHandler),
          Arbiter.Receive<sonar.SonarSensorUpdate>(true,
           _sonarNotifyPort, SonarHandler),
          Arbiter.Receive<sound.SoundSensorUpdate>(true,
           _soundNotifyPort, SoundHandler)
          )
     )
);

We also need to add code that starts the watchdog timer by posting an update to the main port. Before this happens, we need to initialize the watchdog state variable with the current date and time. The following code can be added below the current subscriptions in the Start method:

// Initialize the WatchDog State with the current time
// and start the timer
_state.MostRecentNotification = DateTime.Now;
_mainPort.Post(new WatchDogUpdate(new
         WatchDogUpdateRequest(DateTime.Now)));

The final thing to add is the handler for the watchdog update. This is named WatchDogUpdateHandler and is represented in the following code:

private void WatchDogUpdateHandler(WatchDogUpdate update)
{
    // Get the time span since the last notification from a sensor
    TimeSpan sinceNotification = update.Body.TimeStamp –
             _state.MostRecentNotification;

    // See if the latest notification was received over 30 seconds ago
    if (sinceNotification.TotalSeconds >= 30)
    {
        // Stop the robot
        SpawnIterator<double, double>(0, 0, DriveRobot);
        LogInfo("Last sensor reading: " + _state.MostRecentNotification);
    }

    // Start back the timer to fire every 500 milliseconds
    Activate(
       Arbiter.Receive(false, TimeoutPort(500), delegate(DateTime ts)
          {
           _mainPort.Post(new WatchDogUpdate(
               new WatchDogUpdateRequest(ts)));
           }
        )
    );

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

The WatchDogUpdateHandler first computes the time span since the last sensor notification. If this value is greater than 30 seconds, it stops the robot. Regardless, it reactivates the watchdog update by using a TimeoutPort set to expire in 500 milliseconds. This represents the amount of time until the WatchDogUpdateHandler is called again.

Just like with version 1, we need to alter the manifest for this version. Once again, it is best to use the DSS Manifest Editor to do this. Remember, you will have to first successfully compile the new version of the Wander service before opening the DSS Manifest Editor. After you do this, you can locate the Wander service in the list of services and drag it onto the design surface. You should see entries for the following partners: NxtTouchSensor, NxtDrive, NxtButtons, NxtUltrasonicSensor, and NxtSoundSensor. You need to drag an NXT-specific service onto the box located next to each of these partners.

The process is similar to what was done for the last version. This time you have two more partners to deal with: the sonar and sound sensors. When complete, the Manifest Editor should look like Figure 6-7. Make sure you save a copy of the manifest and replace the one that already exists in the application directory for Wander service version 2.

Figure 6-7. The manifest for version 2 of the Wander service, as it appears in the DSS Manifest Editor.

If you run version 2 of the Wander service, you should notice some immediate differences. Most importantly, the robot should not collide with an object before it turns to move away from the obstacle. Try experimenting with different obstacles to get an idea of how sensitive the NXT sonar sensor is. You may have to adjust the threshold value used in the SonarHandler. Keep in mind that not every object will be detected by the sonar sensor, so the bumper sensor might still have to be used.

By incorporating the sound sensor and watchdog update timer, you have tools in place that can stop your robot in case of an emergency. A loud clapping noise should trigger the sound sensor and, in case the wheels get stuck, the robot should shut down after 30 seconds if there is no feedback from the sensors.