private sicklrf.State _laserData = null; // not part of the state, but still accessible from all components
#endregion Fields
#region Methods
/// <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);
}
/// <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);
}
