void DropHandler(DsspDefaultDrop drop)
{
Tracer.Trace("SickLRF::DropHandler()");
try
{
if (_link != null)
{
// release dispatcher queue resource
ResourceManagerPort.Post(new FreeExecutionResource(_link.TaskQueue));
_link.Close();
_link = null;
}
}
finally
{
base.DefaultDropHandler(drop);
}
}
IEnumerator <ITask> ExceptionHandler(Exception exception)
{
Tracer.Trace("SickLRF::ExceptionHandler() exception=" + exception);
LogError(exception);
BadPacketException bpe = exception as BadPacketException;
if (bpe != null && bpe.Count < 2)
{
yield break;
}
_subMgrPort.Post(new submgr.Submit(new ResetType(), DsspActions.SubmitRequest));
LogInfo("Closing link to LRF");
yield return
(Arbiter.Choice(
_link.Close(),
delegate(SuccessResult success)
{
},
delegate(Exception except)
{
LogError(except);
}
));
_state.LinkState = "LRF Link closed, waiting 5 seconds";
LogInfo(_state.LinkState);
_link = null;
SpawnIterator(5000, _state.ComPort, StartLRF);
yield break;
}
/// <summary>
/// Start conversation with the SickLRF device.
/// </summary>
IEnumerator <ITask> StartLRF(int timeout, int comPort)
{
Tracer.Trace("SickLRF::StartLRF() comPort=" + comPort);
if (timeout > 0)
{
//
// caller asked us to wait <timeout> milliseconds until we start.
//
yield return(Arbiter.Receive(false, TimeoutPort(timeout),
delegate(DateTime dt)
{
LogInfo("Done Waiting");
}
));
}
if (_queue == null)
{
//
// The internal services run on their own dispatcher, we need to create that (once)
//
AllocateExecutionResource allocExecRes = new AllocateExecutionResource(0, "SickLRF");
ResourceManagerPort.Post(allocExecRes);
yield return(Arbiter.Choice(
allocExecRes.Result,
delegate(ExecutionAllocationResult result)
{
_queue = result.TaskQueue;
},
delegate(Exception e)
{
LogError(e);
}
));
}
string comName;
if (comPort <= 0)
{
//
// We default to COM4, because
// a) that was our previous behavior and
// b) the hardware that we have uses COM4
//
comName = "COM4";
}
else
{
comName = "COM" + comPort;
}
_link = new CommLink(_queue ?? TaskQueue, comName, _internalPort);
_link.Parent = ServiceInfo.Service;
_link.Console = ConsoleOutputPort;
FlushPortSet(_internalPort);
yield return(
Arbiter.Choice(
_link.Open(),
delegate(SuccessResult success)
{
LogInfo("Opened link to LRF");
},
delegate(Exception exception)
{
LogError(exception);
}
)
);
}
