/// <summary>
/// Handles the reset notification of the Laser partner.
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderResetUpdate"/> to itself.</remarks>
/// <param name="reset">notification</param>
void LaserResetNotification(sicklrf.Reset reset)
{
_mainPort.Post(new LaserRangeFinderResetUpdate(reset.Body));
}
// TT Nov-2006 - Changed for new CTP
// public void ReplaceLaserData(sicklrf.StateType stateType)
public void ReplaceLaserData(sicklrf.State stateType)
{
if (stateType.TimeStamp < _lastLaser)
{
return;
}
_lastLaser = stateType.TimeStamp;
TimeSpan delay = DateTime.Now - stateType.TimeStamp;
lblDelay.Text = delay.ToString();
// TT - Changes supplied by Ben Axelrod to highlight
// any objects immediately in front of the robot made
// the following code redundant
/*
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 = LinearColor(Color.DarkBlue, Color.LightGray, 0, 8192, range);
g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
}
}
*/
// TT - May-2007
// Added an option to display a map instead of 3D view
if (options.DisplayMap)
{
// Display a top-down map using the colour convention
// for an occupancy grid
picLRF.Image = DrawMap(stateType);
}
else
{
// Display the simulated 3D view
picLRF.Image = Draw3DView(stateType);
}
if (btnConnectLRF.Enabled)
{
btnConnectLRF.Enabled = false;
}
if (!btnDisconnect.Enabled)
{
btnDisconnect.Enabled = true;
}
}
/// <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> LaserReplaceNotification(sicklrf.Replace 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 request = new LaserRangeFinderUpdate(laserData);
_mainPort.Post(request);
yield return Arbiter.Choice(
request.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 data by now.
GetMostRecentLaserNotification(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator<sicklrf.Replace>(false, _laserNotify, LaserReplaceNotification)
);
}
public void ReplaceHandler(sicklrf.Replace replace)
{
Tracer.Trace("TrackRoamerUsrfService::ReplaceHandler()");
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
}
// ====================================================================================
// as generated:
//[ServiceHandler]
//public void SubscribeHandler(sicklrf.Subscribe subscribe)
//{
// throw new NotImplementedException();
//}
/// <summary>
/// Handles Subscribe requests
/// </summary>
/// <param name="subscribe">request message</param>
IEnumerator<ITask> SubscribeHandler(sicklrf.Subscribe subscribe)
{
Tracer.Trace("TrackRoamerUsrfService::SubscribeHandler()");
yield return Arbiter.Choice(
SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
if (_state != null &&
_state.DistanceMeasurements != null)
{
_submgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null));
}
},
null
);
}
/// <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);
}
}
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 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));
}
}
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;
}
}
IEnumerator<ITask> OnLaserReplaceHandler(sicklrf.Replace replace)
{
if (_driveControl != null)
{
WinFormsServicePort.FormInvoke(
delegate()
{
_driveControl.ReplaceLaserData(replace.Body);
}
);
}
LogObject(replace.Body);
yield break;
}
/// <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;
// Why does AdjustVelocity call Turn() ???
// This does not seem to make any sense, and the robot gets
// stuck in oscillations to the left and right
int test = FindBestFrom(laserData, 0, _state.Velocity / 10, CorridorWidthMoving);
if (distance > FreeDistance)
{
MoveForward(MaximumForwardVelocity);
//if (Math.Abs(test) < 10)
if (Math.Abs(test) < 10)
{
Turn(test / 2);
// TT Jun-2007 - Changed to slow down
_state.Countdown = Math.Abs((test / 2) / 5);
}
}
else if (distance > AwareOfObstacleDistance)
{
MoveForward(MaximumForwardVelocity / 2);
//if (Math.Abs(test) < 45)
if (Math.Abs(test) < 45)
{
Turn(test / 2);
// TT Jun-2007 - Changed
_state.Countdown = Math.Abs((test / 2) / 5);
}
}
else
{
MoveForward(MaximumForwardVelocity / 4);
if (Math.Abs(test) < 60)
{
Turn(test);
// TT Jun-2007 - Changed
//_state.Countdown = Math.Abs(test / 10);
_state.Countdown = 5;
}
}
}
/// <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;
// TT - There is a timing issue during startup whereby the
// LRF actually does not supply any data! Just ignore it.
if (laserData == null || laserData.DistanceMeasurements == null)
return 0;
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];
// TT - The simulated LRF returns zero if there is
// no hit. It should return max range. I reported
// this and it should be fixed in V1.5.
if (range == 0)
range = 8192;
int x = (int)(range * Math.Sin(angle));
int y = (int)(range * Math.Cos(angle));
if (Math.Abs(x) < width)
{
if (range < best)
{
best = range;
}
}
}
}
// TT - Log the value
//LogInfo("Nearest Obstacle: " + best);
return best;
}
// TT May-2007 - Draw an occupancy grid style map using the LRF data
private Bitmap DrawMap(sicklrf.State stateType)
{
double angle;
int range;
Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, LRFImageHeight);
Graphics g = Graphics.FromImage(bmp);
g.Clear(Color.LightGray);
// Loop invariants
double startAngle = stateType.AngularRange / 2.0;
// AngularResolution is zero! This is a bug that I have
// reported and it should be fixed in V1.5. In the meantime,
// calculate the value.
double angleIncrement = (double)stateType.AngularRange / (double)stateType.DistanceMeasurements.Length;
double rx, ry;
int ix, iy;
// Pen whitePen = new Pen(Color.White);
// Pen blackPen = new Pen(Color.Black, 2);
// blackPen.EndCap = LineCap.Round;
// First draw the free space
// Note that this code draws the free space using a series
// of white "rays" from the LRF. The result is some small
// "slivers" that are not filled in between the rays.
// Some people fill in the whole area outlined by the
// hit points on the assumption that if two rays are close
// together then there is probably nothing in between.
// Strictly speaking this is not correct because the LRF
// tells you nothing about the spaces between rays.
for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
{
range = stateType.DistanceMeasurements[x];
// NOTE: Scans are backwards, i.e. right to left
angle = (startAngle - x * angleIncrement) * Math.PI / 180;
// The Simulated LRF returns zero if there is no hit
// This is not the way that a real LRF works
if (range <= 0)
range = (int) options.MaxLRFRange;
rx = range * Math.Cos(angle);
ry = range * Math.Sin(angle);
ix = (int)(ry * LRFImageHeight / options.MaxLRFRange) + bmp.Width / 2;
iy = bmp.Height - (int)(rx * LRFImageHeight / options.MaxLRFRange);
// NOTE: This code relies on the fact that the DrawLine
// method will clip at the edges of the bitmap!
g.DrawLine(Pens.White, bmp.Width / 2, bmp.Height - 1, ix, iy);
}
// Now draw the obstacles at the points of the laser hits
for (int x = 0; x < stateType.DistanceMeasurements.Length; x++)
{
range = stateType.DistanceMeasurements[x];
// NOTE: Scans are backwards, i.e. right to left
angle = (startAngle - x * angleIncrement) * Math.PI / 180;
// The Simulated LRF returns zero if there is no hit
// This is not the way that a real LRF works
if (range <= 0)
range = (int)options.MaxLRFRange;
rx = range * Math.Cos(angle);
ry = range * Math.Sin(angle);
ix = (int)(ry * LRFImageHeight / options.MaxLRFRange) + bmp.Width / 2;
iy = bmp.Height - (int)(rx * LRFImageHeight / options.MaxLRFRange);
if (range < options.MaxLRFRange)
{
/*
// We need a small dot at the end of the laser ray
// However, drawing a point with DrawLine does not work!
// So make it a little longer
int ix2, iy2;
ix2 = ix;
if (ix2 > bmp.Width / 2)
ix2 = ix - 1;
if (ix2 < bmp.Width / 2)
ix2 = ix + 1;
iy2 = iy;
if (iy2 < bmp.Height / 2)
iy2 = iy + 1;
g.DrawLine(blackPen, ix, iy, ix2, iy2);
*/
// Put a single pixel at the end of the ray
// This does not give a nice dark boundary like
// drawing a short line, but it is more correct
// because technically the laser only gives data
// at discrete angles and you can't say anything
// about the area in between rays
// NOTE: We have to check the bounds of the image
// for SetPixel -- it does not clip
if (ix >=0 && ix < bmp.Width &&
iy >=0 && iy < bmp.Height)
bmp.SetPixel(ix, iy, Color.Black);
}
}
// Return the image
return bmp;
}
/// <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>
/// 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>
/// 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;
}
}
/// <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);
}
/// <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 (doCountdown && _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);
}
}
/// <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);
}
/// <summary>
/// Copies 'distanceMeasurements' paramater to Bitmap contained in the _lrfPanel
/// </summary>
/// <param name="distanceMeasurements"></param>
/// <param name="units"></param>
internal void SetLRFData(int[] distanceMeasurements, sicklrf.Units units)
{
if (_lrfPanel == null || distanceMeasurements == null)
return;
double lrfMax = 0;
int maxIndex = distanceMeasurements.Length - 1;
float scalingFactor = maxIndex / (float)(_lrfPanel.Width - 1);
byte[] bmpData = new byte[4 * _lrfPanel.Width * _lrfPanel.Height];
for (int y = 0; y < _lrfPanel.Height; y++)
{
for (int x = 0; x < _lrfPanel.Width; x++)
{
int i = 4 * (y * _lrfPanel.Width + x);
int originalDistance = distanceMeasurements[maxIndex - (int)(x * scalingFactor)];
lrfMax = Math.Max(originalDistance, lrfMax);
double distance = (255.0 / _previousLrfMax) * originalDistance;
bmpData[i] = ClampToByte(distance);
}
}
_previousLrfMax = Math.Max(lrfMax, double.Epsilon);
lrflog.Add(_previousLrfMax);
Bitmap bmp = LRFBitmap;
ImageProcessingResultService.CopyBytesToBitmap(bmpData, _lrfPanel.Width, _lrfPanel.Height, ref bmp);
if (bmp != LRFBitmap)
{
LRFBitmap = bmp;
_lrfPanel.Image = LRFBitmap;
}
}
/// <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 ResetHandler(sicklrf.Reset reset)
{
Tracer.Trace("TrackRoamerUsrfService::ResetHandler()");
// TBD: send reset request to the board partner
reset.ResponsePort.Post(DefaultSubmitResponseType.Instance);
}
/// <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 GetHandler(sicklrf.Get get)
{
Tracer.Trace("TrackRoamerUsrfService::GetHandler()");
get.ResponsePort.Post(_state);
}
// TT - Draw the pseudo 3D view including Ben's code to show
// the width of the robot
private Bitmap Draw3DView(sicklrf.State stateType)
{
// TT - Some minor rearrangement of Ben's code
int robotWidth = (int)options.RobotWidth; // mm
double angle, obsThresh;
Color col;
Bitmap bmp = new Bitmap(stateType.DistanceMeasurements.Length, LRFImageHeight);
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 < options.MaxLRFRange)
{
int h = bmp.Height * 500 / stateType.DistanceMeasurements[x];
if (h < 0)
{
h = 0;
}
// TT - Ignore the robot width if it is not set
if (robotWidth > 0)
{
angle = x * Math.PI * stateType.AngularRange /
stateType.DistanceMeasurements.Length / 180; // radians
obsThresh = Math.Abs(robotWidth / (2 * Math.Cos(angle)));
}
else
obsThresh = 0.0;
if (range < obsThresh)
col = LinearColor(Color.DarkRed, Color.LightGray, 0, (int)options.MaxLRFRange, range);
else
col = LinearColor(Color.DarkBlue, Color.LightGray, 0, (int)options.MaxLRFRange, range);
g.DrawLine(new Pen(col), bmp.Width - x, half - h, bmp.Width - x, half + h);
}
}
// Return the image
return bmp;
}
