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