public IEnumerator <ITask> ReplaceHandler(sonar.Replace replace)
{
_state = replace.Body;
if (replace.ResponsePort != null)
{
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
}
// issue notification
_subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
yield break;
}
void CreateDefaultState()
{
_state = new sonar.SonarState
{
AngularRange = 30,
AngularResolution = 0.5f,
DistanceMeasurement = SONAR_RANGE,
DistanceMeasurements = null,
HardwareIdentifier = 0,
MaxDistance = SONAR_RANGE,
Pose = new Pose(),
TimeStamp = DateTime.Now
};
}
/// <summary>
/// Configure sensors
/// </summary>
/// <returns>A CCR iterator</returns>
private IEnumerator <ITask> ConfigureSensors()
{
int i = 0;
// get each sensor's current state
// and assign hardware identifiers
var replaceSensorState = new sonar.Replace();
var get = new sonar.Get();
foreach (var sensor in this.sensorPorts)
{
this.sensorPorts[i].Post(get);
yield return(get.ResponsePort.Choice());
sonar.SonarState sensorState = get.ResponsePort;
if (sensorState != null)
{
replaceSensorState.Body = sensorState;
}
else
{
replaceSensorState.Body = new sonar.SonarState();
}
replaceSensorState.Body.HardwareIdentifier = i;
sensor.Post(replaceSensorState);
i++;
}
ICollection <Fault> faults = null;
yield return(replaceSensorState.ResponsePort.MultipleItemReceive(
this.sensorPorts.Length,
(successes, f) =>
{
faults = f;
}));
if (faults != null && faults.Count > 0)
{
LogError("Failure configuring IR sensors");
this.mainPort.Post(new DsspDefaultDrop());
}
this.StartAfterConfigure();
}
