/// <summary>
/// Handles Replace notifications from the Laser partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator <ITask> LaserReplaceNotification(sicklrf.Replace replace)
{
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
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) { }
));
// Skip messages that have been queued up in the meantime.
// The notification that are lingering are out of data by now.
GetMostRecentLaserNotification(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator <sicklrf.Replace>(false, _laserNotify, LaserReplaceNotification)
);
}
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
{
sicklrf.State laserData = update.Body;
_state.MostRecentLaser = laserData.TimeStamp;
int distance = FindNearestObstacleInCorridor(laserData, CorridorWidthMapping, 45);
// AvoidCollision and EnterOpenSpace have precedence over
// all other state transitions and are thus handled first.
AvoidCollision(distance);
EnterOpenSpace(distance);
UpdateLogicalState(laserData, distance);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
