// Raul - Sonar update handler
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void SonarUpdateHandler(SonarUpdate update)
{
pxsonar.SonarState sonarData = update.Body;
_state.SonarData = sonarData;
_state.MostRecentSonar = sonarData.TimeStamp;
// Raul - Allow one second, so the maze is completely built. Otherwise the sonar seems to go though the walls.
TimeSpan sinceInit = _state.MostRecentSonar - _state.InitTimeStamp;
if (sinceInit.Seconds > 4)
{
if (_state.DrawMap && _state.X != 0 && _state.Y != 0)
{
SpawnIterator(sonarData.AngularRange, sonarData.DistanceMeasurements, UpdateMap);
}
double distance = FindNearestObstacleInCorridor(sonarData, CorridorWidthMapping, 45);
// AvoidCollision and EnterOpenSpace have precedence over
// all other state transitions and are thus handled first.
AvoidCollision(distance);
EnterOpenSpace(distance);
UpdateLogicalState(sonarData, distance);
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
// Raul - Sonar replace notification
/// <summary>
/// Handles Replace notifications from the Sonar partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator<ITask> SonarReplaceNotification(pxsonar.Replace replace)
{
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
pxsonar.SonarState sonarData = replace.Body;
SonarUpdate request = new SonarUpdate(sonarData);
_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.
GetMostRecentSonarNotification(sonarData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator<pxsonar.Replace>(false, _sonarNotify, SonarReplaceNotification)
);
}
