Esempio n. 1
0
        public virtual IEnumerator <ITask> HttpGetSonarSensorHandler(dssphttp.HttpGet httpGet)
        {
            sonararray.Get get = new sonararray.Get();
            this.altPort.Post(get);
            yield return(get.ResponsePort.Choice());

            sonararray.SonarSensorArrayState sonararrayState = get.ResponsePort;
            httpGet.ResponsePort.Post(new dssphttp.HttpResponseType(sonararrayState));
            yield break;
        }
Esempio 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;
        }
Esempio n. 3
0
        /// <summary>
        /// Process the Sensor data
        /// </summary>
        /// <returns>An iterator</returns>
        private IEnumerator <ITask> ProcessSensors()
        {
            if (this.batteryPort == null)
            {
                WinFormsServicePort.FormInvoke(() => this.dashboardForm.UpdateBatteryLevel(null));
            }

            DateTime?lastBatteryPoll = null;

            while (true)
            {
                DateTime now = DateTime.Now;
                if (this.irsensorArrayPort != null)
                {
                    // Create a request that includes a Diagnostic Header so that it can be picked
                    // up by selective logging. If no logging is required, the code would just be:
                    // yield return this.irsensorArrayPort.Get().Choice(
                    infraredsensorarray.Get getRequest = new infraredsensorarray.Get();
                    getRequest.AddHeader(new DiagnosticsHeader());
                    this.irsensorArrayPort.Post(getRequest);
                    yield return(getRequest.ResponsePort.Choice(
                                     s =>
                    {
                        var irValues = new double[Constants.IRSensorCount];
                        for (var i = 0; i < Constants.IRSensorCount; i++)
                        {
                            irValues[Constants.IRSensorCount - i - 1] = s.Sensors[i].DistanceMeasurement * ConvertMetersToCm;
                        }

                        WinFormsServicePort.FormInvoke(() => this.dashboardForm.UpdateIRSensors(irValues));
                    },
                                     LogError));
                }

                if (this.sonarSensorArrayPort != null)
                {
                    // Create a request that includes a Diagnostic Header so that it can be picked
                    // up by selective logging. If no logging is required, the code would just be:
                    // yield return this.sonarSensorArrayPort.Get().Choice(
                    sonarsensorarray.Get getRequest = new sonarsensorarray.Get();
                    getRequest.AddHeader(new DiagnosticsHeader());
                    this.sonarSensorArrayPort.Post(getRequest);
                    yield return(getRequest.ResponsePort.Choice(
                                     s =>
                    {
                        var sonarValues = new double[Constants.SonarSensorCount];
                        for (int i = 0; i < Constants.SonarSensorCount; i++)
                        {
                            sonarValues[Constants.SonarSensorCount - i - 1] = s.Sensors[i].DistanceMeasurement * ConvertMetersToCm;
                        }

                        WinFormsServicePort.FormInvoke(() => this.dashboardForm.UpdateSonarSensors(sonarValues));
                    },
                                     LogError));
                }

                if (this.drivePort != null)
                {
                    yield return(this.drivePort.Get().Choice(
                                     s => WinFormsServicePort.FormInvoke(() => this.dashboardForm.UpdateWheelState(s)),
                                     this.LogError));
                }

                // Only poll the battery every minute
                if (this.batteryPort != null &&
                    (!lastBatteryPoll.HasValue ||
                     DateTime.Now.Subtract(lastBatteryPoll.Value) > TimeSpan.FromMinutes(1)))
                {
                    yield return
                        (this.batteryPort.Get().Choice(
                             s => WinFormsServicePort.FormInvoke(() => this.dashboardForm.UpdateBatteryLevel(s)),
                             this.LogError));

                    lastBatteryPoll = DateTime.Now;
                }

                // Just sleep for the remainder of our sensor polling timout
                int delay = Math.Max(0, (int)(this.sensorPollingInterval - (DateTime.Now - now).TotalMilliseconds));
                yield return(TimeoutPort(delay).Receive());
            }
        }