public void ReplaceHandler(sicklrf.Replace replace)
{
Tracer.Trace("TrackRoamerUsrfService::ReplaceHandler()");
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
}
/// <summary>
/// Handles Replace notifications from the Laser partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator <ITask> LaserReplaceNotificationHandler(sicklrf.Replace replace)
{
//Tracer.Trace("LaserReplaceNotificationHandler() - Replace");
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
sicklrf.State laserData = GetMostRecentLaserNotification(replace.Body);
LaserRangeFinderUpdate laserUpdate = new LaserRangeFinderUpdate(laserData);
_mainPort.Post(laserUpdate); // calls LaserRangeFinderUpdateHandler() with laserUpdate
yield return(Arbiter.Choice(
laserUpdate.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { }
));
// Skip messages that have been queued up in the meantime.
// The notification that are lingering are out of date by now.
GetMostRecentLaserNotification(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator <sicklrf.Replace>(false, _laserNotify, LaserReplaceNotificationHandler)
);
yield break;
}
public IEnumerator <ITask> ReplaceHandler(sicklrf.Replace replace)
{
_state = replace.Body;
if (replace.ResponsePort != null)
{
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
}
// issue notification
_subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
yield break;
}
IEnumerator <ITask> OnLaserReplaceHandler(sicklrf.Replace replace)
{
if (_driveControl != null)
{
WinFormsServicePort.FormInvoke(
delegate()
{
_driveControl.ReplaceLaserData(replace.Body);
}
);
}
LogObject(replace.Body);
yield break;
}
private void RaycastResultsHandler(physics.RaycastResult result)
{
// we just receive ray cast information from physics. Currently we just use
// the distance measurement for each impact point reported. However, our simulation
// engine also provides you the material properties so you can decide here to simulate
// scattering, reflections, noise etc.
sicklrf.State latestResults = new sicklrf.State();
latestResults.DistanceMeasurements = new int[result.SampleCount + 1];
int initValue = (int)(LASER_RANGE * 1000f);
for (int i = 0; i < (result.SampleCount + 1); i++)
{
latestResults.DistanceMeasurements[i] = initValue;
}
foreach (physics.RaycastImpactPoint pt in result.ImpactPoints)
{
// the distance to the impact has been pre-calculted from the origin
// and it's in the fourth element of the vector
latestResults.DistanceMeasurements[pt.ReadingIndex] = (int)(pt.Position.W * 1000f);
}
latestResults.AngularRange = (int)Math.Abs(_entity.RaycastProperties.EndAngle - _entity.RaycastProperties.StartAngle);
latestResults.AngularResolution = _entity.RaycastProperties.AngleIncrement;
latestResults.Units = sicklrf.Units.Millimeters;
latestResults.LinkState = "Measurement received";
latestResults.TimeStamp = DateTime.Now;
// send replace message to self
sicklrf.Replace replace = new sicklrf.Replace();
// for perf reasons dont set response port, we are just talking to ourself anyway
replace.ResponsePort = null;
replace.Body = latestResults;
_mainPort.Post(replace);
}
protected void LaserHandler(sicklrf.Replace replace)
{
// Angular Range = r
// Desired Angular Range = rd
// Minimum Angle = amin = (r/2) - (rd/2)
// Maximum Angle = amax = (r/2) + (rd/2)
// Angular Resolution = e
// Minimum measurement = mmin = floor(amin / e)
// Maximum measurement = mmax = ceil(amax / e)
// Assert e > 0
// Assert rd > e
// Total measurements = mtot
// Assert mtot > 1 (or two)
if (replace == null)
{
return;
}
sicklrf.State laserState = replace.Body;
if (laserState == null)
{
return;
}
if (laserState.DistanceMeasurements == null)
{
return;
}
if (laserState.DistanceMeasurements.Length < 2)
{
return;
}
if (laserState.AngularResolution <= 0)
{
return;
}
if ((_halfObstacleAngleRange * 2) <= laserState.AngularResolution)
{
return;
}
// Note: this assumes laserState.AngularRange % 2 = 0
int halfRange = laserState.AngularRange / 2;
double amin = halfRange - _halfObstacleAngleRange;
double amax = halfRange + _halfObstacleAngleRange;
int mmin = (int)Math.Floor(amin / laserState.AngularResolution);
int mmax = (int)Math.Ceiling(amax / laserState.AngularResolution);
// TODO: check that MinimumObstacleRange is less than this
int computedRange = 8000; // 8000 is around the maximum reported value from the sicklrf
if (mmin == mmax)
{
computedRange = laserState.DistanceMeasurements[mmin];
}
else
{
for (int index = mmin; index <= mmax; ++index)
{
computedRange = Math.Min(computedRange, laserState.DistanceMeasurements[index]);
}
}
//Trace.WriteLine("samples: " + (mmax - mmin) + ", range: " + computedRange);
if (computedRange <= _state.MinimumObstacleRange)
{
_soar.Obstacle = true;
}
else
{
_soar.Obstacle = false;
}
}
