///// <summary>
///// </summary>
//public void readMission()
//{
// if (File.Exists(missionFilename))
// {
// using (TextReader reader = new StreamReader(missionFilename))
// {
// string line;
// while ((line = reader.ReadLine()) != null)
// {
// Tracer.Trace(line);
// }
// }
// }
//}
/// <summary>
/// creates a RoutePlan, based on mapper's robotDirection and cells on the geo plane (using _mapper.geoCellAt()); analyses obstacles (busy cells)
/// </summary>
/// <returns></returns>
public RoutePlan planRoute()
{
DateTime started = DateTime.Now;
RoutePlan plan = new RoutePlan();
// note: _mapper.robotDirection.distanceToGoalMeters may contain distance to goal, in which case a shorter leg going straight to and ending at goal wins
if (_mapper.robotDirection.heading.HasValue)
{
double?distanceToGoalMeters = _mapper.robotDirection.distanceToGoalMeters;
lock (this)
{
try
{
cellPaths.Clear();
double sweepAngleHalf = sweepAngleNormal / 2.0d;
double goalBearingRelative = 0.0d; // straight in front is default
if (_mapper.robotDirection.bearing.HasValue)
{
goalBearingRelative = (double)_mapper.robotDirection.turnRelative;
}
if (Math.Abs(goalBearingRelative) > sweepAngleHalf)
{
// robot is pointing away from the goal, make him turn towards the goal first:
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + goalBearingRelative);
plan.legMeters = null;
plan.closestObstacleAlongBestPathMeters = _mapper.robotState.robotLengthMeters / 2.0d;
}
else
{
int nsteps = (int)Math.Round(sweepAngleHalf / raySweepDegrees);
for (int i = -nsteps; i < nsteps; i++)
{
double pathHeadingRelative = raySweepDegrees * i;
Direction dir = new Direction()
{
heading = _mapper.robotDirection.heading, bearingRelative = pathHeadingRelative
}; // related to robot heading;
CellPath cellPath = shootRay(dir);
cellPath.firstHeadingRelative = pathHeadingRelative;
cellPaths.Add(cellPath);
}
// order (low to high) paths based on their length and deviation from the goal bearing - using pathRatingFunction():
CellPath bestPath = cellPaths.OrderBy(c => pathRatingFunction(c, goalBearingRelative, distanceToGoalMeters, sweepAngleHalf)).Last();
bestPath.isBest = true;
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + bestPath.firstHeadingRelative);
plan.legMeters = bestPath.lengthMeters;
plan.closestObstacleAlongBestPathMeters = bestPath.lengthMeters - _mapper.robotState.robotLengthMeters / 2.0d;
}
}
catch (Exception exc)
{
Console.WriteLine("planRoute() - " + exc);
}
}
}
plan.timeSpentPlanning = DateTime.Now - started;
return(plan);
}
/// <summary>
/// Transitions to the most appropriate state.
/// </summary>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void UpdateMovingState(RoutePlan plan)
{
LogInfo("TrackRoamerBehaviorsService:UpdateMovingState() Countdown=" + _state.Countdown + " MovingState=" + _state.MovingState);
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
//if (_state.Countdown > 0)
//{
// _state.Countdown--;
// //_state.Countdown = 0;
//}
//else
// based on MetaState, go to the MetaState...() method - which may change the MovingState appropriately
if (_state.IsUnknown || _state.MovingState == MovingState.Unable)
{
MetaStateStartMapping(plan);
}
else if (_state.IsMoving)
{
MetaStateMove(plan);
}
else if (_state.IsMapping)
{
MetaStateMap(plan);
}
}
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>
/// Transitions to "Mapping" meta state or "AdjustHeading" state depending on
/// environment.
/// </summary>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void MetaStateStartMapping(RoutePlan plan)
{
int distance = 0;
if (plan.legMeters.HasValue)
{
distance = (int)((double)plan.legMeters * 1000.0d);
}
LogInfo("TrackRoamerBehaviorsService: MetaStateStartMapping(distance=" + distance + ") ObstacleDistance=" + ObstacleDistanceMm);
Talker.Say(5, "Mapping");
markNorthBitmap("Start Mapping");
StopMoving();
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
if (distance < ObstacleDistanceMm)
{
LogInfo(" distance < ObstacleDistance (not finding exit) _state.Mapped=" + _state.Mapped);
_state.MovingState = MovingState.Recovering;
LogInfo(" MovingState Recovering (not finding exit)");
//Talker.Say(5, "Recovering");
setMovingStateDetail("Recovering (not finding exit)");
/*
if (_state.Mapped)
{
// We have been mapping before but do not seem to
// have found anything.
_state.MovingState = MovingState.RandomTurn;
LogInfo(" requesting RandomTurn");
Talker.Say(5, "random turn");
setMovingStateDetail("Start Mapping (not finding exit) - requesting RandomTurn");
}
else
{
_state.MovingState = MovingState.MapSurroundings;
LogInfo(" requesting MapSurroundings");
Talker.Say(5, "map surroundings");
setMovingStateDetail("Start Mapping (not finding exit) - requesting MapSurroundings");
}
* */
}
else
{
LogInfo(" distance > ObstacleDistance (exit route found) CorridorWidthMapping=" + CorridorWidthMappingMm);
//Talker.Say(5, "exit route found");
int legMm = Math.Min(ObstacleDistanceMm, distance);
_state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); // right - positive, left - negative
LogInfo("Leg: " + legMm + " mm NewHeading=" + _state.NewHeading);
setMovingStateDetail("Start Mapping (exit route found) NewHeading=" + _state.NewHeading + "Leg: " + legMm + " mm");
_state.MovingState = MovingState.AdjustHeading;
_state.Countdown = legMm / 50 + Math.Abs(_state.NewHeading / 10);
LogInfo(" new Countdown=" + _state.Countdown);
}
}
private const double TurningWaitInterval = 1.0d; // wait for so many seconds after a turn before accepting a laser frame (should be close to full sweep cycle of the laser)
/// <summary>
/// just turn where the plan wants you to; we are calling with good plan (heading) and no legMeters.
/// </summary>
/// <param name="plan"></param>
private void TurnToGoal(RoutePlan plan)
{
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
_state.NewHeading = (int)Math.Round((double)plan.bestHeadingRelative(_mapperVicinity));
setMovingStateDetail("TurnToGoal to NewHeading=" + _state.NewHeading);
AdjustHeading();
}
/// <summary>
/// Implements the "Moving" meta state, basically any transition and not in-place sequence.
/// </summary>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void MetaStateMove(RoutePlan plan)
{
LogInfo("TrackRoamerBehaviorsService: MetaStateMove()");
Talker.Say(4, "Move - " + _state.MovingState);
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
switch (_state.MovingState)
{
case MovingState.AdjustHeading:
setMovingStateDetail("MetaStateMove - AdjustHeading to NewHeading=" + _state.NewHeading);
AdjustHeading();
break;
case MovingState.FreeForwards:
setMovingStateDetail("MetaStateMove - FreeForwards");
AdjustDirectionAndVelocity(plan);
break;
case MovingState.InTransition:
case MovingState.Unknown:
// inTransition means "while executing TurnAndMoveForward() command" (see base).
// TurnAndMoveForward() may for a moment leave the state in "Unknown", do not treat it as termination.
// this allows current transition to complete, without exiting the "Moving" metastate
break;
case MovingState.BumpedBackingUp:
setMovingStateDetail("MetaStateMove - BumpedBackingUp");
break;
default:
LogError("TrackRoamerBehaviorsService:MetaStateMove() called in illegal state - " + _state.MovingState);
break;
}
}
/// <summary>
/// Adjusts the velocity based on environment.
/// </summary>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void AdjustDirectionAndVelocity(RoutePlan plan)
{
LogInfo("TrackRoamerBehaviorsService: AdjustDirectionAndVelocity()");
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
int legDistanceMm = _state.collisionState.canMoveForwardDistanceMm;
if (plan.legMeters.HasValue)
{
legDistanceMm = Math.Min(legDistanceMm, (int)((double)plan.legMeters * 1000.0d));
}
// we come here when _state.MovingState == MovingState.FreeForwards
_state.Mapped = false;
//int bestHeading = FindBestHeading(_laserData, 0, (int)Math.Round(_state.Velocity * 1000.0d / 10.0d), CorridorWidthMoving, "FreeForwards - Adjust");
_state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); // right - positive, left - negative
LogInfo(" bestHeading=" + _state.NewHeading + currentCompass);
if (legDistanceMm > FreeDistanceMm)
{
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor),
rotatePower = MaximumTurnPower,
speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, MaximumForwardVelocityMmSec)),
desiredMovingState = MovingState.FreeForwards
},
delegate()
{
},
TurnAndMoveForward);
}
else if (legDistanceMm > AwareOfObstacleDistanceMm)
{
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor),
rotatePower = ModerateTurnPower,
speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, ModerateForwardVelocityMmSec)),
desiredMovingState = MovingState.FreeForwards
},
delegate()
{
},
TurnAndMoveForward);
}
else
{
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters()
{
rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor),
rotatePower = MinimumTurnPower,
speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, MinimumForwardVelocityMmSec)),
desiredMovingState = MovingState.FreeForwards
},
delegate()
{
_state.Countdown = Math.Abs(_state.NewHeading / 10);
},
TurnAndMoveForward);
}
LogInfo(" new Countdown=" + _state.Countdown);
}
/// <summary>
/// Implements the "Mapping" meta state.
/// </summary>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void MetaStateMap(RoutePlan plan)
{
LogInfo("TrackRoamerBehaviorsService: MetaStateMap() MovingState=" + _state.MovingState);
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
switch (_state.MovingState)
{
case MovingState.RandomTurn:
LogInfo("MovingState.RandomTurn");
setMovingStateDetail("MetaStateMap - performing RandomTurn");
RandomTurn();
break;
case MovingState.Recovering:
LogInfo("MovingState.Recovering");
setMovingStateDetail("MetaStateMap - Recovering");
Recover();
break;
/*
case MovingState.MapSurroundings:
_state.Mapped = true;
LogInfo("MovingState.MapSurroundings: Turning 180 deg to map");
setMovingStateDetail("MetaStateMap - turning 180 deg to map");
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters() {
speed = (int)Math.Round(MaximumBackwardVelocity),
rotatePower = ModerateTurnPower,
rotateAngle = 180,
desiredMovingState = MovingState.MapSouth
},
delegate() {
_state.Countdown = 15;
},
TurnAndMoveForward);
break;
case MovingState.MapSouth:
LogInfo("MovingState.MapSouth: mapping the View South");
Talker.Say(5, "mapping South");
setMovingStateDetail("MetaStateMap - mapping South");
// _state.South = _laserData; // needed for FindBestComposite()
SpawnIterator<TurnAndMoveParameters, Handler>(
new TurnAndMoveParameters() {
speed = (int)Math.Round(MaximumBackwardVelocity),
rotatePower = ModerateTurnPower,
rotateAngle = 180,
desiredMovingState = MovingState.MapNorth
},
delegate() {
_state.Countdown = 15;
},
TurnAndMoveForward);
break;
case MovingState.MapNorth:
LogInfo("MovingState.MapNorth: mapping the View North");
Talker.Say(5, "mapping North");
//_state.NewHeading = FindBestComposite(_state.South, _laserData);
_state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity);
LogInfo("Composite Map suggests turn to heading: " + _state.NewHeading);
setMovingStateDetail("MetaStateMap - mapping North - Composite Map suggests turn: " + _state.NewHeading);
_state.South = null;
_state.MovingState = MovingState.AdjustHeading;
break;
*/
case MovingState.InTransition:
break;
default:
LogError("TrackRoamerBehaviorsService:MetaStateMap() called in illegal state - " + _state.MovingState);
break;
}
}
/// <summary>
/// If the robot is mapping and there is sufficient open space directly ahead, enter this space.
/// </summary>
/// <param name="distance"></param>
private void EnterOpenSpace(RoutePlan plan)
{
if (_state.IsMapping && plan != null && plan.isGoodPlan)
{
// at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// we must use them while executing the plan.
int? distance = (int)((double)plan.closestObstacleAlongBestPathMeters * 1000.0d);
if (distance > SafeDistanceMm)
{
// We are mapping but can see plenty of free space ahead.
// The robot should go into this space.
LogInfo("TrackRoamerBehaviorsService: EnterOpenSpace() - while mapping, distance greater than SafeDistance=" + SafeDistanceMm);
string message = "open space."; // "while mapping - detected open space; entering...";
Talker.Say(5, message);
LogHistory(5, message);
setMovingStateDetail(message);
StopTurning();
_state.MovingState = MovingState.FreeForwards;
_state.Countdown = 4;
}
}
}
/// <summary>
/// evaluates proximity and parking sensor data and translates all of it into
/// actionable restrictions in CollisionState object
/// </summary>
public void computeCollisionState(RoutePlan plan, double? intendedVelocity)
{
CollisionState cs = _state.collisionState;
//cs.initRestrictive();
cs.initPermissive(FreeDistanceMm, MaximumForwardVelocityMmSec, MaximumBackwardVelocityMmSec);
// initialize to the maximums:
canMoveForwardDistanceM = FreeDistanceMm * 2.0d / 1000.0d;
canMoveBackwardsDistanceM = FreeDistanceMm * 2.0d / 1000.0d;
//Tracer.Trace("IR ffl=" + _state.MostRecentProximity.mfl + "m PS MetersLF=" + _state.MostRecentParkingSensor.parkingSensorMetersLF + "m");
if(intendedVelocity == null)
{
intendedVelocity = _state.Velocity;
}
bool isMovingForward = intendedVelocity > 0.01d; // m/s
bool isMovingBackwards = intendedVelocity < -0.01d;
int? distance = null; // mm
if (plan != null && plan.isGoodPlan)
{
double? bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity);
if (bestHeadingRelative != null && bestHeadingRelative.Value > -45.0d && bestHeadingRelative.Value < 45.0d)
{
distance = (int)(plan.legMeters.Value * 1000.0d);
if (distance < ObstacleDistanceMm)
{
cs.canMoveByPlan = false;
}
else
{
cs.canMoveByPlan = true;
}
}
}
DateTime sensorInvalidateTime = DateTime.Now.AddSeconds(-ignoreOldSensorReadingsIntervalSec); // ignore sensor readings that are older than this time
StringBuilder msb = new StringBuilder(); // build the message here
// we compute full state every time, but format the message so it is relevant to the direction we move to.
// forward sensors: -------------------------------------------------------------------------------------------
proxibrick.ProximityDataDssSerializable MostRecentProximity = _state.MostRecentProximity;
proxibrick.ParkingSensorDataDssSerializable MostRecentParkingSensor = _state.MostRecentParkingSensor;
if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime)
{
double mfl = MostRecentProximity.mfl;
if (mfl < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Left Side proximity detected obstacle at {0}m; ", mfl);
}
cs.canTurnLeft &= mfl > proxiTresholdTurn1;
if (mfl < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mfl;
}
}
double mffl = MostRecentProximity.mffl;
if (mffl < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("FF_Left proximity detected obstacle at {0}m; ", mffl);
}
//cs.canTurnLeft &= mffl > proxiTresholdTurn2;
if (mffl < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mffl;
}
}
double mffr = MostRecentProximity.mffr;
if (mffr < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("FF_Right proximity detected obstacle at {0}m; ", mffr);
}
//cs.canTurnRight &= mffr > proxiTresholdTurn2;
if (mffr < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mffr;
}
}
double mfr = MostRecentProximity.mfr;
if (mfr < proxiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Right Side proximity detected obstacle at {0}m; ", mfr);
}
cs.canTurnRight &= mfr > proxiTresholdTurn1;
if (mfr < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = mfr;
}
}
}
if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime)
{
double parkingSensorMetersLF = MostRecentParkingSensor.parkingSensorMetersLF;
if (parkingSensorMetersLF < psiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLF);
}
if (parkingSensorMetersLF < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = parkingSensorMetersLF;
}
}
double parkingSensorMetersRF = MostRecentParkingSensor.parkingSensorMetersRF;
if (parkingSensorMetersRF < psiTreshold)
{
if (isMovingForward)
{
msb.AppendFormat("Front Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRF);
}
if (parkingSensorMetersRF < canMoveForwardDistanceM)
{
canMoveForwardDistanceM = parkingSensorMetersRF;
}
}
}
// backward sensors: -------------------------------------------------------------------------------------------
if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime)
{
double mbl = MostRecentProximity.mbl;
if (mbl < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Left Side proximity detected obstacle at {0}m; ", mbl);
}
cs.canTurnRight &= mbl > proxiTresholdTurn1;
if (mbl < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbl;
}
}
double mbbl = MostRecentProximity.mbbl;
if (mbbl < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("BB_Left proximity detected obstacle at {0}m; ", mbbl);
}
//cs.canTurnRight &= mbbl > proxiTresholdTurn2;
if (mbbl < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbbl;
}
}
double mbbr = MostRecentProximity.mbbr;
if (mbbr < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("BB_Right proximity detected obstacle at {0}m; ", mbbr);
}
//cs.canTurnLeft &= mbbr > proxiTresholdTurn2;
if (mbbr < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbbr;
}
}
double mbr = MostRecentProximity.mbr;
if (mbr < proxiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Right Side proximity detected obstacle at {0}m; ", mbr);
}
cs.canTurnLeft &= mbr > proxiTresholdTurn1;
if (mbr < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = mbr;
}
}
}
if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime)
{
double parkingSensorMetersLB = MostRecentParkingSensor.parkingSensorMetersLB;
if (parkingSensorMetersLB < psiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLB);
}
if (parkingSensorMetersLB < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = parkingSensorMetersLB;
}
}
double parkingSensorMetersRB = MostRecentParkingSensor.parkingSensorMetersRB;
if (parkingSensorMetersRB < psiTreshold)
{
if (isMovingBackwards)
{
msb.AppendFormat("Back Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRB);
}
if (parkingSensorMetersRB < canMoveBackwardsDistanceM)
{
canMoveBackwardsDistanceM = parkingSensorMetersRB;
}
}
}
if (_state.MostRecentWhiskerLeft)
{
msb.Append("Left Whisker; ");
canMoveForwardDistanceM = 0.0d;
cs.canMoveForward = false;
cs.canTurnRight = false;
cs.canTurnLeft = false;
}
if (_state.MostRecentWhiskerRight)
{
msb.Append("Right Whisker; ");
canMoveForwardDistanceM = 0.0d;
cs.canMoveForward = false;
cs.canTurnRight = false;
cs.canTurnLeft = false;
}
adjustNearCollisionSpeed(canMoveForwardDistanceM, true);
adjustNearCollisionSpeed(canMoveBackwardsDistanceM, false);
// compute the "mustStop" and "message":
if (isMovingForward)
{
cs.mustStop = !cs.canMoveForward;
}
else if (isMovingBackwards)
{
cs.mustStop = !cs.canMoveBackwards;
}
if (msb.Length > 0 || cs.mustStop)
{
if (isMovingForward)
{
msb.Insert(0, "While moving forward: ");
}
else if (isMovingBackwards)
{
msb.Insert(0, "While moving backwards: ");
}
if (cs.mustStop)
{
msb.Insert(0, "!! MUST STOP !! ");
}
}
else
{
if (isMovingForward)
{
msb.Append("Can continue forward: ");
}
else if (isMovingBackwards)
{
msb.Append("Can continue backwards: ");
}
else
{
msb.Append("Can move: ");
}
}
msb.AppendFormat(" FWD: {0} to {1} mm at {2:F0} mm/s BKWD: {3} to {4} mm at {5:F0} mm/s LEFT:{6} RIGHT:{7}",
cs.canMoveForward, cs.canMoveForwardDistanceMm, cs.canMoveForwardSpeedMms,
cs.canMoveBackwards, cs.canMoveBackwardsDistanceMm, cs.canMoveBackwardsSpeedMms,
cs.canTurnLeft, cs.canTurnRight);
if (cs.canMoveByPlan.HasValue && !cs.canMoveByPlan.Value)
{
msb.AppendFormat("; best plan obstacle too close at {0} mm; ", distance);
}
string message = msb.ToString();
//if (!string.Equals(message, _lastMessage))
//{
// Tracer.Trace(message);
// _lastMessage = message;
//}
cs.message = message;
}
/// <summary>
/// If the robot is moving or intends to move, and an obstacle is too close, return false. The caller must take care to stop the movement.
/// You can then map the environment for a way around it, or see what options are in _state.collisionState
/// </summary>
/// <param name="distance"></param>
private bool PerformAvoidCollision(RoutePlan plan, double? intendedVelocity = null)
{
computeCollisionState(plan, intendedVelocity);
setCollisionLights();
string messageCollisionState = _state.collisionState.message;
bool mustStop = false;
if (_state.IsMoving)
{
// we may come here with just proximity and other data, but not laser data.
if (_state.collisionState.canMoveByPlan.HasValue && !_state.collisionState.canMoveByPlan.Value)
{
//
// We are moving and there is something less than < ObstacleDistance>
// millimeters from the center of the robot (within field of view) on the planned leg. STOP.
//
LogInfo("TrackRoamerBehaviorsService: AvoidCollision() - best plan obstacle - closer than ObstacleDistance=" + ObstacleDistanceMm);
mustStop = true;
}
}
setMovingStateDetail(messageCollisionState);
if (_state.collisionState.mustStop || mustStop)
{
//string messageToSay = "stop."; // "avoid collision - must stop";
//if ((DateTime.Now - lastStopAnnounced).TotalMilliseconds > 3000.0d)
//{
// Talker.Say(5, messageToSay);
// lastStopAnnounced = DateTime.Now;
//}
LogHistory(5, "avoid collision - must stop");
return false; // has to stop
}
else
{
return true; // does not have to stop
}
}
/// <summary>
/// creates a RoutePlan, based on mapper's robotDirection and cells on the geo plane (using _mapper.geoCellAt()); analyses obstacles (busy cells)
/// </summary>
/// <returns></returns>
public RoutePlan planRoute()
{
DateTime started = DateTime.Now;
RoutePlan plan = new RoutePlan();
if (_mapper.robotDirection.heading.HasValue)
{
lock (this)
{
try
{
cellPaths.Clear();
double sweepAngleHalf = sweepAngleNormal / 2.0d;
double goalBearingRelative = 0.0d; // straight in front is default
if (_mapper.robotDirection.bearing.HasValue)
{
goalBearingRelative = (double)_mapper.robotDirection.turnRelative;
}
if (Math.Abs(goalBearingRelative) > sweepAngleHalf)
{
// robot is pointing away from the goal, make him turn towards the goal first:
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + goalBearingRelative);
plan.legMeters = null;
plan.closestObstacleMeters = _mapper.robotState.robotLengthMeters / 2.0d;
}
else
{
int nsteps = (int)Math.Round(sweepAngleHalf / raySweepDegrees);
for (int i = -nsteps; i < nsteps; i++)
{
double pathHeadingRelative = raySweepDegrees * i;
Direction dir = new Direction() { heading = _mapper.robotDirection.heading, bearingRelative = pathHeadingRelative }; // related to robot heading;
CellPath cellPath = shootRay(dir);
cellPath.firstHeadingRelative = pathHeadingRelative;
cellPaths.Add(cellPath);
}
// order (low to high) paths based on their length and deviation from the goal bearing - using pathRatingFunction():
CellPath bestPath = cellPaths.OrderBy(c => pathRatingFunction(c, goalBearingRelative, sweepAngleHalf)).Last();
bestPath.isBest = true;
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + bestPath.firstHeadingRelative);
plan.legMeters = bestPath.lengthMeters;
plan.closestObstacleMeters = bestPath.lengthMeters - _mapper.robotState.robotLengthMeters;
}
}
catch (Exception exc)
{
Console.WriteLine("planRoute() - " + exc);
}
}
}
plan.timeSpentPlanning = DateTime.Now - started;
return plan;
}
