/// <summary> /// Samples sensors used for obstacle avoidance /// </summary> /// <param name="timestamp">Sampling timestamp</param> /// <returns>CCR ITask enumerator</returns> private IEnumerator<ITask> SampleSensors(DateTime timestamp) { var startTime = Utilities.ElapsedSecondsSinceStart; try { if (!this.CheckIfValidPendingDriveRequest(this.pendingSetDrivePower)) { yield break; } // If this is a command to move in reverse, then execute it // and skip obstacle avoidance if (this.pendingSetDrivePower.LeftWheelPower < 0 && this.pendingSetDrivePower.RightWheelPower < 0) { this.SetPowerWithAcceleration(this.pendingSetDrivePower.LeftWheelPower, this.pendingSetDrivePower.RightWheelPower); yield break; } PortSet<infraredsensorarray.InfraredSensorArrayState, Fault> infraredGetResponsePort = null; PortSet<sonarsensorarray.SonarSensorArrayState, Fault> sonarGetResponsePort = null; // sample proximity depth cam sensor in paraller // Sample depthcam. Set timeout so we dont wait forever var getDepthCamState = new depthcam.Get(); getDepthCamState.TimeSpan = TimeSpan.FromSeconds(SamplingIntervalInSeconds * 5); this.depthCameraPort.Post(getDepthCamState); if (this.HasInfraredSensors) { // issue ir sensor array get infraredGetResponsePort = this.infraredSensorArrayPort.Get(); } if (this.HasSonarSensors) { // issue sonar sensor array get sonarGetResponsePort = this.sonarSensorArrayPort.Get(); } yield return getDepthCamState.ResponsePort.Choice(); depthcam.DepthCamSensorState depthCamState = getDepthCamState.ResponsePort; if (depthCamState == null) { LogError((Fault)getDepthCamState.ResponsePort); yield break; } if (this.floorCeilingMinDepths == null) { this.InitializeDepthProcessing(depthCamState.InverseProjectionMatrix); } var depthImage = depthCamState.DepthImage; if (depthCamState.DepthImageSize.Width > DefaultDepthCamImageWidth || depthCamState.DepthImageSize.Height > DefaultDepthcamImageHeight) { depthImage = this.DownSampleDepthImage(depthCamState); } else { depthImage = depthCamState.DepthImage.Clone() as short[]; } // remove floor and ceiling int floorPixelCount; DepthImageUtilities.FilterOutGroundAndAboveRobotValues( floorHoleAsObstacleDepthThresholdAlongDepthAxisInMillimeters: 5000, floorDetectionMarginInMillimeters: 200, preFilterNoReadingDepthValue: (int)NoReading, postFilterNoReadingDepthValue: (int)MaxValidDepth, floorDepthValue: (int)Floor, minValidDepthMillimeters: (short)MinValidDepth, maxValidDepthMillimeters: (short)MaxValidDepth, depthData: depthImage, floorCeilingDepths: this.floorCeilingMaxDepths, numberOfFloorPixels: out floorPixelCount); var minDepthProfile = DepthImageUtilities.CalculateHorizontalDepthProfileAsShortArray( depthImage, DefaultDepthCamImageWidth, DefaultDepthcamImageHeight, (short)NoReading, (short)Floor, DeadZoneColumnCount, (int)MinValidDepth, (int)MaxValidDepth, DefaultDepthCamImageWidth); // Cache the minDepthProfile before ir sensor fusion so we can track changes made via sensor fusion. // The cached profile can be visualized via the HttpQuery handler to see how sensors are contributing to the depth profile short[] temp = minDepthProfile.Clone() as short[]; this.irSensorProfile = new short[minDepthProfile.Length]; if (this.HasInfraredSensors) { yield return infraredGetResponsePort.Choice(); infraredsensorarray.InfraredSensorArrayState infraredState = infraredGetResponsePort; // A fault during the Get will cause this to be null if (infraredState != null) { this.FuseDepthProfileWithIrReadings(depthCamState.FieldOfView, infraredState, minDepthProfile); for (int i = 0; i < minDepthProfile.Length; i++) { if (minDepthProfile[i] != temp[i]) { this.irSensorProfile[i] = minDepthProfile[i]; } } } } // cache the minDepthProfile before sonar sensor fusion so we can track changes made via sensor fusion. // The cached profile can be visualized via the HttpQuery handler to see how sensors are contributing to the depth profile temp = minDepthProfile.Clone() as short[]; this.sonarSensorProfile = new short[minDepthProfile.Length]; if (this.HasSonarSensors) { yield return sonarGetResponsePort.Choice(); sonarsensorarray.SonarSensorArrayState sonarState = sonarGetResponsePort; // A fault during the Get will cause this to be null if (sonarState != null) { this.FuseDepthProfileWithSonarReadings(depthCamState.FieldOfView, sonarState, minDepthProfile); for (int i = 0; i < minDepthProfile.Length; i++) { if (minDepthProfile[i] != temp[i]) { this.sonarSensorProfile[i] = minDepthProfile[i]; } } } } this.depthProfile = minDepthProfile.Clone() as short[]; this.depthDownSampleBuffer = depthImage.Clone() as short[]; // Asynchronously update depth profile image Spawn(this.UpdateDepthProfileImage); this.AvoidObstacles(minDepthProfile, depthCamState.MaximumRange); } finally { var endTime = Utilities.ElapsedSecondsSinceStart; var elapsed = endTime - startTime; var remainderToNextSamplingTime = SamplingIntervalInSeconds - elapsed; if (remainderToNextSamplingTime <= 0) { // schedule immediately remainderToNextSamplingTime = 0.015; } if (this.ServicePhase == ServiceRuntimePhase.Started) { // schedule next sampling interval Activate(this.TimeoutPort( TimeSpan.FromSeconds(remainderToNextSamplingTime)).Receive( (dt) => this.samplePort.Post(dt))); } } }
public IEnumerator <ITask> GetHandler(depthcam.Get get) { get.ResponsePort.Post(_state); yield break; }
public void OnGet(depth.Get get) { get.ResponsePort.Post(this.depthCamState); }