Ejemplo n.º 1
0
        /// <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();
        }
Ejemplo n.º 2
0
        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;
 }