/// <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();
}
public IEnumerator <ITask> GetSonarArrayHandler(sonararray.Get get)
{
var state = new sonararray.SonarSensorArrayState();
state.Sensors = new List <sonarsensor.SonarState>(this.sensorPorts.Length);
// compose state on the fly by issuing N parallel GET requests to partners
var getSensorState = new sonar.Get();
foreach (var port in this.sensorPorts)
{
state.Sensors.Add(null);
port.Post(getSensorState);
}
ICollection <sonar.SonarState> sensorStates = null;
ICollection <Fault> faults = null;
yield return(getSensorState.ResponsePort.MultipleItemReceive(
this.sensorPorts.Length,
(s, f) =>
{
sensorStates = s;
faults = f;
}));
if (faults != null && faults.Count > 0)
{
get.ResponsePort.Post(Fault.FromCodeSubcode(FaultCodes.Receiver, DsspFaultCodes.OperationFailed));
yield break;
}
foreach (var sensorState in sensorStates)
{
state.Sensors[sensorState.HardwareIdentifier] = new sonarsensor.SonarState()
{
HardwareIdentifier = sensorState.HardwareIdentifier,
AngularRange = sensorState.AngularRange,
AngularResolution = sensorState.AngularResolution,
DistanceMeasurement = sensorState.DistanceMeasurement,
DistanceMeasurements = sensorState.DistanceMeasurements,
MaxDistance = sensorState.MaxDistance,
Pose = new PhysicalModel.Proxy.Pose
{
Orientation = new PhysicalModel.Proxy.Quaternion(
sensorState.Pose.Orientation.X,
sensorState.Pose.Orientation.Y,
sensorState.Pose.Orientation.Z,
sensorState.Pose.Orientation.W),
Position = new PhysicalModel.Proxy.Vector3(
sensorState.Pose.Position.X,
sensorState.Pose.Position.Y,
sensorState.Pose.Position.Z),
},
TimeStamp = sensorState.TimeStamp,
};
}
var stateClone = state.Clone() as sonararray.SonarSensorArrayState;
get.ResponsePort.Post(stateClone);
yield break;
}
public IEnumerator <ITask> GetHandler(sonar.Get get)
{
_state.DistanceMeasurement = _entity.LatestReading;
get.ResponsePort.Post(_state);
yield break;
}
