/// <summary>
/// Adjusts the velocity based on environment.
/// </summary>
/// <param name="laserData">most recently sensed laser range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void AdjustVelocity(sicklrf.State laserData, int distance)
{
_state.Mapped = false;
int test = FindBestFrom(laserData, 0, _state.Velocity / 10, CorridorWidthMoving);
if (distance > FreeDistance)
{
MoveForward(MaximumForwardVelocity);
if (Math.Abs(test) < 10)
{
Turn(test / 2);
}
}
else if (distance > AwareOfObstacleDistance)
{
MoveForward(MaximumForwardVelocity / 2);
if (Math.Abs(test) < 45)
{
Turn(test / 2);
}
}
else
{
MoveForward(MaximumForwardVelocity / 4);
Turn(test);
_state.Countdown = Math.Abs(test / 10);
}
}
/// <summary>
/// Transitions to "Mapping" meta state or "AdjustHeading" state depending on
/// environment.
/// </summary>
/// <param name="laserData">most recently sensed laser range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void StartMapping(sicklrf.State laserData, int distance)
{
StopMoving();
if (distance < ObstacleDistance)
{
if (_state.Mapped)
{
// We have been mapping before but do not seem to
// have found anything.
_state.LogicalState = LogicalState.RandomTurn;
}
else
{
_state.LogicalState = LogicalState.MapSurroundings;
}
}
else
{
int step = Math.Min(ObstacleDistance, distance - CorridorWidthMapping);
// find the best angle from step mm in front of
// our current position
_state.NewHeading = FindBestFrom(laserData, 0, step, CorridorWidthMapping);
LogInfo("Step: " + step + " Turn: " + _state.NewHeading);
Translate(step);
_state.LogicalState = LogicalState.AdjustHeading;
_state.Countdown = step / 50 + Math.Abs(_state.NewHeading / 10);
}
}
/// <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;
}
/// <summary>
/// Finds closest obstacle in a corridor.
/// </summary>
/// <param name="laserData">laser scan</param>
/// <param name="width">corridor width</param>
/// <param name="fov">field of view in degrees</param>
/// <returns>distance to the closest obstacle</returns>
private int FindNearestObstacleInCorridor(sicklrf.State laserData, int width, int fov)
{
int index;
int best = 8192;
int count = laserData.DistanceMeasurements.Length;
double rangeLow = -laserData.AngularRange / 2.0;
double rangeHigh = laserData.AngularRange / 2.0;
double span = laserData.AngularRange;
for (index = 0; index < count; index++)
{
double angle = rangeLow + (span * index) / count;
if (Math.Abs(angle) < fov)
{
angle = angle * Math.PI / 180;
int range = laserData.DistanceMeasurements[index];
int x = (int)(range * Math.Sin(angle));
int y = (int)(range * Math.Cos(angle));
if (Math.Abs(x) < width)
{
if (range < best)
{
best = range;
}
}
}
}
return(best);
}
public void ReplaceHandler(sicklrf.Replace replace)
{
Tracer.Trace("TrackRoamerUsrfService::ReplaceHandler()");
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
}
/// <summary>
/// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
/// </summary>
/// <param name="south">the backward half of the scan</param>
/// <param name="north">the forward half of the scan</param>
/// <returns>beast heading in degrees</returns>
private int FindBestComposite(sicklrf.State south, sicklrf.State north)
{
sicklrf.State composite = new sicklrf.State();
composite.DistanceMeasurements = new int[720];
for (int i = 0; i < 720; i++)
{
if (i < 180)
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i + 180];
}
else if (i < 540)
{
composite.DistanceMeasurements[i] = north.DistanceMeasurements[i - 180];
}
else
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i - 540];
}
}
composite.AngularResolution = 0.5;
composite.AngularRange = 360;
composite.Units = north.Units;
return(FindBestFrom(composite, 0, 0, CorridorWidthMoving));
}
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;
}
private void createCylinder(sicklrf.State stateType)
{
Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
Graphics g = Graphics.FromImage(bmp);
g.Clear(Color.LightGray);
int half = bmp.Height / 2;
for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
{
int range = stateType.DistanceMeasurements[x];
//for red marking
double angle = x * Math.PI * stateType.AngularRange / stateType.DistanceMeasurements.Length / 180; //radians
double obsThresh = Math.Abs(robotWidth / (2 * Math.Cos(angle)));
if (range > 0 && range < maxDist)
{
int h = (int)Math.Round(bmp.Height * 500.0 / range);
if (h < 0)
{
h = 0;
}
Color col;
if (range < obsThresh)
{
col = LinearColor(Color.DarkRed, Color.LightGray, 0, maxDist, range);
}
else
{
col = LinearColor(Color.DarkBlue, Color.LightGray, 0, maxDist, range);
}
g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
}
int hlim = (int)Math.Round(bmp.Height * 500.0 / obsThresh);
if (hlim < 0)
{
hlim = 0;
}
if (hlim < half)
{
bmp.SetPixel(bmp.Width - x, half + hlim, Color.Green);
}
}
picLRF.Image = bmp;
}
private const int step = 6; // for speed and given that we are actually dealing with sonar data, pick only Nth points.
protected void updateMapperWithLaserData(sicklrf.State laserData)
{
int numRays = laserData.DistanceMeasurements.Length;
List <IDetectedObject> laserObjects = new List <IDetectedObject>(numRays / step + 5);
for (int i = 0; i < laserData.DistanceMeasurements.Length; i += step)
{
double rangeMeters = laserData.DistanceMeasurements[i] / 1000.0d; // DistanceMeasurements is in millimeters;
if (rangeMeters > minReliableRangeMeters && rangeMeters < maxReliableRangeMeters)
{
double relBearing = forwardAngle - i * 180.0d / numRays;
GeoPosition pos1 = (GeoPosition)_mapperVicinity.robotPosition.Clone();
pos1.translate(new Direction()
{
heading = _mapperVicinity.robotDirection.heading, bearingRelative = relBearing
}, new Distance(rangeMeters));
DetectedObstacle dobst1 = new DetectedObstacle()
{
geoPosition = pos1,
firstSeen = laserData.TimeStamp.Ticks,
lastSeen = laserData.TimeStamp.Ticks,
detectorType = DetectorType.SONAR_SCANNING,
objectType = DetectedObjectType.Obstacle
};
dobst1.SetColorByType();
laserObjects.Add(dobst1);
}
}
if (laserObjects.Count > 0)
{
int countBefore = 0;
lock (_mapperVicinity)
{
countBefore = _mapperVicinity.Count;
_mapperVicinity.AddRange(laserObjects);
_mapperVicinity.computeMapPositions();
}
//Tracer.Trace(string.Format("laser ready - added {0} to {1}", laserObjects.Count, countBefore));
}
}
/// <summary>
/// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
/// </summary>
/// <param name="south">the backward half of the scan</param>
/// <param name="north">the forward half of the scan</param>
/// <returns>best heading in degrees (right turn - positive, left turn - negative)</returns>
private int FindBestComposite(sicklrf.State south, sicklrf.State north)
{
try
{
// sanity check:
LogInfo("FindBestComposite() south: " + south.DistanceMeasurements.Length + " points, north: " + north.DistanceMeasurements.Length + " points");
if (south.DistanceMeasurements.Length + north.DistanceMeasurements.Length < 360)
{
LogError("FindBestComposite() - bad laser measurement");
return(0);
}
sicklrf.State composite = new sicklrf.State();
composite.DistanceMeasurements = new int[361];
for (int i = 0; i < composite.DistanceMeasurements.Length; i++)
{
// the trick is to have halves of the South scan become sides of the full North scan,
// so that a 360 degrees composite has heading 0 in the middle.
if (i < 90)
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i + 90];
}
else if (i < 270)
{
composite.DistanceMeasurements[i] = north.DistanceMeasurements[i - 90];
}
else // 270...359
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i - 270];
}
}
composite.AngularResolution = 1.0;
composite.AngularRange = 360;
composite.Units = north.Units;
return(FindBestHeading(composite, 0, 0, CorridorWidthMoving, "Best Composite"));
}
catch (Exception exc)
{
LogError(exc);
return(0);
}
}
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
{
sicklrf.State laserData = update.Body;
_state.MostRecentLaser = laserData.TimeStamp;
int distance = FindNearestObstacleInCorridor(laserData, CorridorWidthMapping, 45);
// AvoidCollision and EnterOpenSpace have precedence over
// all other state transitions and are thus handled first.
AvoidCollision(distance);
EnterOpenSpace(distance);
UpdateLogicalState(laserData, distance);
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
IEnumerator <ITask> UpdateLRF()
{
var sensorOrFault = _simulatedLRFServicePort.Get();
yield return(sensorOrFault.Choice());
if (!HasError(sensorOrFault))
{
sicklrf.State sensorState = (sicklrf.State)sensorOrFault;
WinFormsServicePort.Post(new FormInvoke(() =>
{
_imageProcessingForm.SetLRFData(sensorState.DistanceMeasurements, sensorState.Units);
}));
}
yield break;
}
/// <summary>
/// Implements the "Moving" meta state.
/// </summary>
/// <param name="laserData">most recently sensed laser range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void Move(sicklrf.State laserData, int distance)
{
switch (_state.LogicalState)
{
case LogicalState.AdjustHeading:
AdjustHeading();
break;
case LogicalState.FreeForwards:
AdjustVelocity(laserData, distance);
break;
default:
LogInfo("Explorer.Move() called in illegal state");
break;
}
}
private sicklrf.State _laserData = null; // not part of the state, but still accessible from all components
#region Laser handlers
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
protected void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
{
//Tracer.Trace("LaserRangeFinderUpdateHandler() - Update");
try
{
if (!_doSimulatedLaser) // if simulated, ignore real data - do not call Decide()
{
_laserData = (sicklrf.State)update.Body.Clone(); // laserData.DistanceMeasurements is cloned here all right
_state.MostRecentLaserTimeStamp = _laserData.TimeStamp;
updateMapperWithOdometryData();
if (!_mapperVicinity.robotState.ignoreLaser) // if asked to ignore laser, we just do not update mapper with obstacles, but still call Decide() and everything else
{
updateMapperWithLaserData(_laserData);
}
else
{
lock (_mapperVicinity)
{
_mapperVicinity.computeMapPositions();
}
}
if (!_testBumpMode && !_state.Dropping && !_doUnitTest)
{
Decide(SensorEventSource.LaserScanning);
}
setGuiCurrentLaserData(new LaserDataSerializable()
{
TimeStamp = _laserData.TimeStamp.Ticks, DistanceMeasurements = (int[])_laserData.DistanceMeasurements.Clone()
});
}
}
catch (Exception exc)
{
Tracer.Trace("LaserRangeFinderUpdateHandler() - " + exc);
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// Transitions to the most appropriate state.
/// </summary>
/// <param name="laserData">most recently sensed laser range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void UpdateLogicalState(sicklrf.State laserData, int distance)
{
if (_state.Countdown > 0)
{
_state.Countdown--;
}
else if (_state.IsUnknown)
{
StartMapping(laserData, distance);
}
else if (_state.IsMoving)
{
Move(laserData, distance);
}
else if (_state.IsMapping)
{
Map(laserData, distance);
}
}
public void ReplaceLaserData(sicklrf.State stateType)
{
if (stateType.TimeStamp < _lastLaser)
{
return;
}
_lastLaser = stateType.TimeStamp;
TimeSpan delay = DateTime.Now - stateType.TimeStamp;
lblDelay.Text = delay.ToString();
Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, 100);
Graphics g = Graphics.FromImage(bmp);
g.Clear(Color.LightGray);
int half = bmp.Height / 2;
for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
{
int range = stateType.DistanceMeasurements[x];
if (range > 0 && range < 8192)
{
int h = bmp.Height * 500 / stateType.DistanceMeasurements[x];
if (h < 0)
{
h = 0;
}
Color col = ColorHelper.LinearColor(Color.DarkBlue, Color.LightGray, 0, 8192, range);
g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
}
}
picLRF.Image = bmp;
if (btnConnectLRF.Enabled)
{
btnConnectLRF.Enabled = false;
}
if (!btnDisconnect.Enabled)
{
btnDisconnect.Enabled = true;
}
}
private sicklrf.State _laserData = null; // not part of the state, but still accessible from all components
#endregion Fields
#region Methods
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
protected void LaserRangeFinderUpdateHandler(LaserRangeFinderUpdate update)
{
//Tracer.Trace("LaserRangeFinderUpdateHandler() - Update");
try
{
if (!_doSimulatedLaser) // if simulated, ignore real data - do not call Decide()
{
_laserData = (sicklrf.State)update.Body.Clone(); // laserData.DistanceMeasurements is cloned here all right
_state.MostRecentLaserTimeStamp = _laserData.TimeStamp;
updateMapperWithOdometryData();
if (!_mapperVicinity.robotState.ignoreLaser) // if asked to ignore laser, we just do not update mapper with obstacles, but still call Decide() and everything else
{
updateMapperWithLaserData(_laserData);
}
else
{
lock (_mapperVicinity)
{
_mapperVicinity.computeMapPositions();
}
}
if (!_testBumpMode && !_state.Dropping && !_doUnitTest)
{
Decide(SensorEventSource.LaserScanning);
}
setGuiCurrentLaserData(new LaserDataSerializable() { TimeStamp = _laserData.TimeStamp.Ticks, DistanceMeasurements = (int[])_laserData.DistanceMeasurements.Clone() });
}
}
catch (Exception exc)
{
Tracer.Trace("LaserRangeFinderUpdateHandler() - " + exc);
}
update.ResponsePort.Post(DefaultUpdateResponseType.Instance);
}
/// <summary>
/// Implements the "Mapping" meta state.
/// </summary>
/// <param name="laserData">most recently sensed laser range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void Map(sicklrf.State laserData, int distance)
{
switch (_state.LogicalState)
{
case LogicalState.RandomTurn:
RandomTurn();
break;
case LogicalState.MapSurroundings:
_state.Mapped = true;
LogInfo("Turning 180 deg to map");
Turn(180);
_state.LogicalState = LogicalState.MapSouth;
_state.Countdown = 15;
break;
case LogicalState.MapSouth:
LogInfo("Mapping the View South");
_state.South = laserData;
Turn(180);
_state.LogicalState = LogicalState.MapNorth;
_state.Countdown = 15;
break;
case LogicalState.MapNorth:
LogInfo("Mapping the View North");
_state.NewHeading = FindBestComposite(_state.South, laserData);
LogInfo("Map suggest turn: " + _state.NewHeading);
_state.South = null;
_state.LogicalState = LogicalState.AdjustHeading;
break;
default:
LogInfo("Explorer.Map() called in illegal state");
break;
}
}
/// <summary>
/// Gets the most recent laser notification. Older notifications are dropped.
/// </summary>
/// <param name="laserData">last known laser data</param>
/// <returns>most recent laser data</returns>
private sicklrf.State GetMostRecentLaserNotification(sicklrf.State laserData)
{
sicklrf.Replace testReplace;
// _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
// the portset contains
int count = _laserNotify.P3.ItemCount - 1;
for (int i = 0; i < count; i++)
{
testReplace = _laserNotify.Test <sicklrf.Replace>();
if (testReplace.Body.TimeStamp > laserData.TimeStamp)
{
laserData = testReplace.Body;
}
}
if (count > 0)
{
LogInfo(string.Format("Dropped {0} laser readings (laser start)", count));
}
return(laserData);
}
public void ReplaceLaserData(sicklrf.State stateType)
{
if (stateType.TimeStamp < _lastLaser)
{
return;
}
_lastLaser = stateType.TimeStamp;
TimeSpan delay = DateTime.Now - stateType.TimeStamp;
lblDelay.Text = delay.ToString();
createCylinder(stateType);
createTop(stateType);
if (btnConnectLRF.Enabled)
{
btnConnectLRF.Enabled = false;
}
if (!btnDisconnect.Enabled)
{
btnDisconnect.Enabled = true;
}
}
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);
}
/// <summary>
/// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
/// </summary>
/// <param name="south">the backward half of the scan</param>
/// <param name="north">the forward half of the scan</param>
/// <returns>beast heading in degrees</returns>
private int FindBestComposite(sicklrf.State south, sicklrf.State north)
{
sicklrf.State composite = new sicklrf.State();
composite.DistanceMeasurements = new int[720];
for (int i = 0; i < 720; i++)
{
if (i < 180)
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i + 180];
}
else if (i < 540)
{
composite.DistanceMeasurements[i] = north.DistanceMeasurements[i - 180];
}
else
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i - 540];
}
}
composite.AngularResolution = 0.5;
composite.AngularRange = 360;
composite.Units = north.Units;
return FindBestFrom(composite, 0, 0, CorridorWidthMoving);
}
public LaserRangeFinderUpdate(sicklrf.State body)
: base(body)
{
}
protected void GenerateTop(sicklrf.State laserData, int fieldOfView, RoutePlan plan)
{
//lock (lockStatusGraphics)
//{
if (currentStatusGraphics != null)
{
bool haveLaser = laserData != null && laserData.DistanceMeasurements != null && (DateTime.Now - _laserData.TimeStamp).TotalSeconds < 2.0d;
//Bitmap bmp = (_state.MovingState == MovingState.MapSouth) ? currentStatusGraphics.statusBmp : currentStatusGraphics.northBmp;
//Bitmap bmp = currentStatusGraphics.statusBmp;
Bitmap bmp = currentStatusGraphics.northBmp;
lock (bmp)
{
using (Graphics g = Graphics.FromImage(bmp))
{
g.Clear(Color.LightGray);
List <Lbl> labels = new List <Lbl>();
if (haveLaser)
{
double angularOffset = -90 + laserData.AngularRange / 2.0;
double piBy180 = Math.PI / 180.0;
double halfAngle = laserData.AngularResolution / 2.0;
double drangeMax = 0.0d;
GraphicsPath path = new GraphicsPath();
// make two passes, drawing laser data and label every 20th range:
for (int pass = 1; pass <= 2; pass++)
{
for (int i = 0; i < laserData.DistanceMeasurements.Length; i++)
{
int range = laserData.DistanceMeasurements[i];
if (range > 0 && range < 8192)
{
double angle = i * laserData.AngularResolution - angularOffset;
double lowAngle = (angle - halfAngle) * piBy180;
double highAngle = (angle + halfAngle) * piBy180;
double drange = range * StatusGraphics.scale;
float lx = (float)(StatusGraphics.xCenter + drange * Math.Cos(lowAngle));
float ly = (float)(StatusGraphics.xCenter - drange * Math.Sin(lowAngle));
float hx = (float)(StatusGraphics.xCenter + drange * Math.Cos(highAngle));
float hy = (float)(StatusGraphics.xCenter - drange * Math.Sin(highAngle));
if (pass == 1)
{
// on the first pass just add lines to the Path and calculate the max range:
if (i == 0)
{
path.AddLine(StatusGraphics.xCenter, StatusGraphics.imageHeight, lx, ly);
}
path.AddLine(lx, ly, hx, hy);
drangeMax = Math.Max(drangeMax, drange);
}
else
{
// on the second pass draw the perimeter and label every 20th range:
g.DrawLine(Pens.DarkBlue, lx, ly, hx, hy);
if (i > 0 && i % 20 == 0 && i < laserData.DistanceMeasurements.Length - 10)
{
float llx = (float)(StatusGraphics.xCenter + drangeMax * 1.3f * Math.Cos(lowAngle));
float lly = (float)(StatusGraphics.xCenter - drangeMax * 1.3f * Math.Sin(lowAngle));
double roundRange = Math.Round(range / 1000.0d, 1); // meters
string str = "" + roundRange;
labels.Add(new Lbl()
{
label = str, lx = llx, ly = lly, brush = Brushes.Black
});
}
}
}
}
if (pass == 1)
{
// draw the laser sweep on the first pass:
g.FillPath(Brushes.White, path);
}
}
}
// draw important decision-influencing boundaries:
float startAngle = -150.0f;
float sweepAngle = 120.0f;
// the "stop moving" distance:
float radius = (float)(ObstacleDistanceMm * StatusGraphics.scale);
//g.DrawRectangle(Pens.Red, xCenter - radius, imageHeight - radius, radius * 2, radius * 2);
g.DrawArc(Pens.Red, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle);
rayLabel("stop", labels, ObstacleDistanceMm, -60.0d, Brushes.Red);
// the "slow down" distance:
radius = (float)(AwareOfObstacleDistanceMm * StatusGraphics.scale);
g.DrawArc(Pens.Orange, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle);
rayLabel("slow", labels, AwareOfObstacleDistanceMm, -60.0d, Brushes.Orange);
// the "stop mapping, enter the open space" distance
radius = (float)(SafeDistanceMm * StatusGraphics.scale);
g.DrawArc(Pens.LightBlue, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle);
rayLabel("enter", labels, SafeDistanceMm, -60.0d, Brushes.LightBlue);
// the "max velocity" distance:
radius = (float)(FreeDistanceMm * StatusGraphics.scale);
g.DrawArc(Pens.Green, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle);
rayLabel("free", labels, FreeDistanceMm, -60.0d, Brushes.Green);
// the fieldOfView arc:
radius = (float)(StatusGraphics.imageHeight - 10);
g.DrawArc(Pens.LimeGreen, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, (float)(-90 - fieldOfView), (float)(fieldOfView * 2));
rayLabel("field of view", labels, (StatusGraphics.imageHeight + 5) / StatusGraphics.scale, 10.0d, Brushes.LimeGreen);
// now draw the robot. Trackroamer is 680 mm wide.
float botHalfWidth = (float)(680 / 2.0d * StatusGraphics.scale);
DrawHelper.drawRobotBoundaries(g, botHalfWidth, StatusGraphics.imageWidth / 2, StatusGraphics.imageHeight);
// debug- draw a ray pointing to predefined direction; left is positive, right is negative:
//double rayLength = 2000.0d;
//rayLine("20", g, rayLength, 20.0d, Pens.Cyan, Brushes.DarkCyan);
//rayLine("-20", g, rayLength, -20.0d, Pens.Blue, Brushes.Blue);
//rayLine("70", g, rayLength, 70.0d, Pens.Red, Brushes.Red);
//rayLine("-70", g, rayLength, -70.0d, Pens.Green, Brushes.Green);
// draw laser's time stamp:
if (haveLaser)
{
TimeSpan howOld = DateTime.Now - laserData.TimeStamp;
g.DrawString(laserData.TimeStamp.ToString() + " (" + howOld + ")", StatusGraphics.fontBmp, Brushes.Black, 0, 0);
}
HistoryItem latestDecisionsHistory = StatusGraphics.HistoryDecisions.Peek();
HistoryItem latestSaidHistory = Talker.HistorySaid.Peek();
if (latestSaidHistory != null)
{
g.DrawString(latestSaidHistory.message, StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 + 80, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 20);
}
if (latestDecisionsHistory != null)
{
g.DrawString(latestDecisionsHistory.message, StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 + 80, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 40);
}
g.DrawString(_state.MovingState.ToString(), StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 - 40, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 20);
// draw distance labels over all other graphics:
foreach (Lbl lbl in labels)
{
g.DrawString(lbl.label, StatusGraphics.fontBmp, lbl.brush, lbl.lx, lbl.ly + 20);
}
// a 200W x 400H rectangle:
Rectangle drawRect = new Rectangle(StatusGraphics.imageWidth / 2 - StatusGraphics.extraHeight, StatusGraphics.imageHeight - StatusGraphics.extraHeight * 2, StatusGraphics.extraHeight * 2, StatusGraphics.extraHeight * 4);
if (_state.MostRecentProximity != null)
{
// the MostRecentProximity class here comes from the proxy, and does not have arrangedForDrawing member. Restore it:
double[] arrangedForDrawing = new double[8];
arrangedForDrawing[0] = _state.MostRecentProximity.mbr;
arrangedForDrawing[1] = _state.MostRecentProximity.mbbr;
arrangedForDrawing[2] = _state.MostRecentProximity.mbbl;
arrangedForDrawing[3] = _state.MostRecentProximity.mbl;
arrangedForDrawing[4] = _state.MostRecentProximity.mfr;
arrangedForDrawing[5] = _state.MostRecentProximity.mffr;
arrangedForDrawing[6] = _state.MostRecentProximity.mffl;
arrangedForDrawing[7] = _state.MostRecentProximity.mfl;
// draw a 200x400 image of IR proximity sensors:
DrawHelper.drawProximityVectors(g, drawRect, arrangedForDrawing, 1);
}
if (_state.MostRecentParkingSensor != null)
{
// the MostRecentParkingSensor class here comes from the proxy, and does not have arrangedForDrawing member. Restore it:
double[] arrangedForDrawing = new double[4];
arrangedForDrawing[0] = _state.MostRecentParkingSensor.parkingSensorMetersRB;
arrangedForDrawing[1] = _state.MostRecentParkingSensor.parkingSensorMetersLB;
arrangedForDrawing[2] = _state.MostRecentParkingSensor.parkingSensorMetersLF;
arrangedForDrawing[3] = _state.MostRecentParkingSensor.parkingSensorMetersRF;
// draw a 200x400 image of parking sensors:
DrawHelper.drawProximityVectors(g, drawRect, arrangedForDrawing, 2);
}
if (plan != null && plan.isGoodPlan)
{
// right turn - positive, left turn - negative, expressed in degrees:
int bestHeadingInt = (int)Math.Round((double)plan.bestHeadingRelative(_mapperVicinity));
double bestHeadingDistance = plan.legMeters.Value * 1000.0d;
using (Pen dirPen = new Pen(Color.Green, 5.0f))
{
dirPen.EndCap = LineCap.ArrowAnchor;
rayLine("best:" + bestHeadingInt, g, bestHeadingDistance * 1.1d, (double)bestHeadingInt, dirPen, Brushes.Green);
}
//int xLbl = 10;
//int yLbl = bmp.Height - 40;
//g.DrawString(comment, StatusGraphics.fontBmpL, Brushes.Green, xLbl, yLbl);
}
if (_mapperVicinity.robotDirection.bearingRelative.HasValue)
{
int goalBearingInt = (int)Math.Round((double)_mapperVicinity.robotDirection.bearingRelative);
double goalDistance = 3000.0d;
using (Pen dirPen = new Pen(Color.LimeGreen, 2.0f))
{
dirPen.EndCap = LineCap.ArrowAnchor;
rayLine("goal:" + goalBearingInt, g, goalDistance, (double)goalBearingInt, dirPen, Brushes.LimeGreen);
}
}
}
}
}
}
/// <summary>
/// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
/// </summary>
/// <param name="south">the backward half of the scan</param>
/// <param name="north">the forward half of the scan</param>
/// <returns>best heading in degrees (right turn - positive, left turn - negative)</returns>
private int FindBestComposite(sicklrf.State south, sicklrf.State north)
{
try
{
// sanity check:
LogInfo("FindBestComposite() south: " + south.DistanceMeasurements.Length + " points, north: " + north.DistanceMeasurements.Length + " points");
if (south.DistanceMeasurements.Length + north.DistanceMeasurements.Length < 360)
{
LogError("FindBestComposite() - bad laser measurement");
return 0;
}
sicklrf.State composite = new sicklrf.State();
composite.DistanceMeasurements = new int[361];
for (int i = 0; i < composite.DistanceMeasurements.Length; i++)
{
// the trick is to have halves of the South scan become sides of the full North scan,
// so that a 360 degrees composite has heading 0 in the middle.
if (i < 90)
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i + 90];
}
else if (i < 270)
{
composite.DistanceMeasurements[i] = north.DistanceMeasurements[i - 90];
}
else // 270...359
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i - 270];
}
}
composite.AngularResolution = 1.0;
composite.AngularRange = 360;
composite.Units = north.Units;
return FindBestHeading(composite, 0, 0, CorridorWidthMoving, "Best Composite");
}
catch (Exception exc)
{
LogError(exc);
return 0;
}
}
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;
}
}
private void createTop(sicklrf.State stateType)
{
int height = 200;
int width = 400;
Bitmap bmp = new Bitmap(width, height);
Graphics g = Graphics.FromImage(bmp);
g.Clear(Color.LightGray);
double angularOffset = -90 + stateType.AngularRange / 2.0;
double piBy180 = Math.PI / 180.0;
double halfAngle = stateType.AngularRange / 360.0 / 2.0;
double scale = (double)height / (double)maxDist;
GraphicsPath path = new GraphicsPath();
for (int pass = 0; pass != 2; pass++)
{
for (int i = 0; i < stateType.DistanceMeasurements.Length; i++)
{
int range = stateType.DistanceMeasurements[i];
if (range <= 0)
{
range = maxDist;
}
double angle = i * (stateType.AngularRange / 360.0) - angularOffset;
double lowAngle = (angle - halfAngle) * piBy180;
double highAngle = (angle + halfAngle) * piBy180;
double drange = range * scale;
float lx = (float)(height + drange * Math.Cos(lowAngle));
float ly = (float)(height - drange * Math.Sin(lowAngle));
float hx = (float)(height + drange * Math.Cos(highAngle));
float hy = (float)(height - drange * Math.Sin(highAngle));
if (pass == 0)
{
if (i == 0)
{
path.AddLine(height, height, lx, ly);
}
path.AddLine(lx, ly, hx, hy);
}
else
{
if (range < maxDist)
{
double myangle = i * Math.PI * stateType.AngularRange / stateType.DistanceMeasurements.Length / 180; //radians
double obsThresh = Math.Abs(robotWidth / (2 * Math.Cos(myangle)));
if (range < obsThresh)
{
g.DrawLine(Pens.Red, lx, ly, hx, hy);
}
else
{
g.DrawLine(Pens.DarkBlue, lx, ly, hx, hy);
}
}
}
}
if (pass == 0)
{
g.FillPath(Brushes.White, path);
}
}
float botWidth = (float)((robotWidth / 2.0) * scale);
g.DrawLine(Pens.Red, height, height - botWidth, height, height);
g.DrawLine(Pens.Red, height - 3, height - botWidth, height + 3, height - botWidth);
g.DrawLine(Pens.Red, height - botWidth, height - 3, height - botWidth, height);
g.DrawLine(Pens.Red, height + botWidth, height - 3, height + botWidth, height);
g.DrawLine(Pens.Red, height - botWidth, height - 1, height + botWidth, height - 1);
g.DrawString(
stateType.TimeStamp.ToString(),
new Font(FontFamily.GenericSansSerif, 10, GraphicsUnit.Pixel),
Brushes.Gray, 0, 0
);
picLRFtop.Image = bmp;
}
/// <summary>
/// Finds the best heading in a 180 degree laser scan
/// </summary>
/// <param name="laserData">laser scan</param>
/// <param name="dx">horizontal offset</param>
/// <param name="dy">vertical offset</param>
/// <param name="width">width of corridor that must be free</param>
/// <returns>best heading in degrees</returns>
private int FindBestFrom(sicklrf.State laserData, int dx, int dy, int width)
{
int count = laserData.DistanceMeasurements.Length;
double span = Math.PI * laserData.AngularRange / 180.0;
List <RangeData> ranges = new List <RangeData>();
for (int i = 0; i < count; i++)
{
int range = laserData.DistanceMeasurements[i];
double angle = span * i / count - span / 2.0;
double x = range * Math.Sin(angle) - dx;
double y = range * Math.Cos(angle) - dy;
angle = Math.Atan2(-x, y);
range = (int)Math.Sqrt(x * x + y * y);
ranges.Add(new RangeData(range, angle));
}
ranges.Sort(RangeData.ByDistance);
for (int i = 0; i < ranges.Count; i++)
{
RangeData curr = ranges[i];
double delta = Math.Atan2(width, curr.Distance);
double low = curr.Heading - delta;
double high = curr.Heading + delta;
for (int j = i + 1; j < ranges.Count; j++)
{
if (ranges[j].Heading > low &&
ranges[j].Heading < high)
{
ranges.RemoveAt(j);
j--;
}
}
}
ranges.Reverse();
int bestDistance = ranges[0].Distance;
double bestHeading = ranges[0].Heading;
Random rand = new Random();
for (int i = 0; i < ranges.Count; i++)
{
if (ranges[i].Distance < bestDistance)
{
break;
}
if (rand.Next(i + 1) == 0)
{
bestHeading = ranges[i].Heading;
}
}
return(-(int)Math.Round(180 * bestHeading / Math.PI));
}
public void ReplaceHandler(sicklrf.Replace replace)
{
Tracer.Trace("TrackRoamerUsrfService::ReplaceHandler()");
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
}
protected sicklrf.State simulatedLaser()
{
sicklrf.State laserData = new sicklrf.State();
laserData.Description = "simulated";
laserData.DistanceMeasurements = new int[181];
for (int i = 0; i < laserData.DistanceMeasurements.Length; i++)
{
laserData.DistanceMeasurements[i] = 3500;
}
laserData.Units = sicklrf.Units.Millimeters;
laserData.AngularResolution = 1.0d;
laserData.AngularRange = 180;
laserData.TimeStamp = DateTime.Now;
_state.MostRecentLaserTimeStamp = laserData.TimeStamp;
return laserData;
}
