/// <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); }