void WatchDogUpdateHandler(WatchDogUpdate update)
{
// Raul - If using LRF
if (_state.UseLRF)
{
TimeSpan sinceLaser = update.Body.TimeStamp - _state.MostRecentLaser;
if (sinceLaser.TotalMilliseconds >= WatchdogTimeout && !_state.IsUnknown)
{
LogInfo("Stop requested, last laser data seen at " + _state.MostRecentLaser);
StopMoving();
_state.LogicalState = LogicalState.Unknown;
}
}
// Raul - If using Sonar
if (_state.UseSonar)
{
TimeSpan sinceSonar = update.Body.TimeStamp - _state.MostRecentSonar;
//if (sinceSonar.TotalMilliseconds >= WatchdogTimeout && !_state.IsUnknown)
//{
// LogInfo("Stop requested, last sonar data seen at " + _state.MostRecentSonar);
// StopMoving();
// _state.LogicalState = LogicalState.Unknown;
//}
}
if (_state.SonarData != null)
{
if (_state.DrawMap && _state.X != 0 && _state.Y != 0)
{
GetMostRecentSonarNotification(_state.SonarData);
SpawnIterator(_state.SonarData.AngularRange, _state.SonarData.DistanceMeasurements, UpdateMap);
}
double distance = FindNearestObstacleInCorridor(_state.SonarData, CorridorWidthMapping, 45);
// AvoidCollision and EnterOpenSpace have precedence over
// all other state transitions and are thus handled first.
AvoidCollision(distance);
EnterOpenSpace(distance);
UpdateLogicalState(_state.SonarData, distance);
}
Activate(
Arbiter.Receive(
false,
TimeoutPort(WatchdogInterval),
delegate(DateTime ts)
{
_mainPort.Post(new WatchDogUpdate(new WatchDogUpdateRequest(ts)));
}
)
);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
void WatchDogUpdateHandler(WatchDogUpdate update)
{
TimeSpan sinceLaser = update.Body.TimeStamp - _state.MostRecentLaser;
LogInfo("Explorer: WatchDogUpdateHandler()");
if (sinceLaser.TotalMilliseconds >= WatchdogTimeout && !_state.IsUnknown)
{
LogInfo("Stop requested, last laser data seen at " + _state.MostRecentLaser);
LogInfo("Stop requested, last laser data seen at " + _state.MostRecentLaser);
StopMoving();
_state.LogicalState = LogicalState.Unknown;
}
Activate(
Arbiter.Receive(
false,
TimeoutPort(WatchdogInterval),
delegate(DateTime ts)
{
_mainPort.Post(new WatchDogUpdate(new WatchDogUpdateRequest(ts)));
}
)
);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
void WatchDogUpdateHandler(WatchDogUpdate update)
{
if (_doSimulatedLaser)
{
if (!_testBumpMode && !_state.Dropping)
{
_laserData = simulatedLaser();
Decide(SensorEventSource.LaserScanning); // we disable real laser events, so we need to call Decide somehow
}
else
{
// avoid errors due to timeout:
_state.MostRecentLaserTimeStamp = DateTime.Now;
}
}
TimeSpan sinceLaser = update.Body.TimeStamp - _state.MostRecentLaserTimeStamp;
//LogInfo("DriveBehaviorServiceBase: WatchDogUpdateHandler()");
if (sinceLaser.TotalMilliseconds >= WatchdogTimeout && !_state.IsUnknown)
{
LogInfo("Stop requested, last laser data seen at " + _state.MostRecentLaserTimeStamp);
StopMoving();
_state.MovingState = MovingState.Unknown;
}
Activate(
Arbiter.Receive(
false,
TimeoutPort(WatchdogInterval),
delegate(DateTime ts)
{
// kick off the timer again for the next cycle
_mainPort.Post(new WatchDogUpdate(new WatchDogUpdateRequest(ts)));
}
)
);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
