/// <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> LaserReplaceNotificationHandler(sicklrf.Replace replace)
{
//Tracer.Trace("LaserReplaceNotificationHandler() - 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 laserUpdate = new LaserRangeFinderUpdate(laserData);
_mainPort.Post(laserUpdate); // calls LaserRangeFinderUpdateHandler() with laserUpdate
yield return(Arbiter.Choice(
laserUpdate.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 date by now.
GetMostRecentLaserNotification(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator <sicklrf.Replace>(false, _laserNotify, LaserReplaceNotificationHandler)
);
yield break;
}
private sicklrf.State _laserData = null; // not part of the state, but still accessible from all components
#region Laser handlers
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
protected void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
{
//Tracer.Trace("LaserRangeFinderUpdateHandler() - Update");
try
{
if (!_doSimulatedLaser) // if simulated, ignore real data - do not call Decide()
{
_laserData = (sicklrf.State)update.Body.Clone(); // laserData.DistanceMeasurements is cloned here all right
_state.MostRecentLaserTimeStamp = _laserData.TimeStamp;
updateMapperWithOdometryData();
if (!_mapperVicinity.robotState.ignoreLaser) // if asked to ignore laser, we just do not update mapper with obstacles, but still call Decide() and everything else
{
updateMapperWithLaserData(_laserData);
}
else
{
lock (_mapperVicinity)
{
_mapperVicinity.computeMapPositions();
}
}
if (!_testBumpMode && !_state.Dropping && !_doUnitTest)
{
Decide(SensorEventSource.LaserScanning);
}
setGuiCurrentLaserData(new LaserDataSerializable()
{
TimeStamp = _laserData.TimeStamp.Ticks, DistanceMeasurements = (int[])_laserData.DistanceMeasurements.Clone()
});
}
}
catch (Exception exc)
{
Tracer.Trace("LaserRangeFinderUpdateHandler() - " + exc);
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
