/// <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; }