private void StrategyRouteFollowing()
{
LocationWp nextWp = _routePlanner.mission.nextTargetWp;
FollowDirectionTargetDistanceToGoalMeters = 0.0d;
if (nextWp != null)
{
Direction dirToWp = nextWp.directionToWp(_mapperVicinity.robotPosition, _mapperVicinity.robotDirection);
Distance distToWp = nextWp.distanceToWp(_mapperVicinity.robotPosition);
if (distToWp.Meters < 2.0d)
{
nextWp.waypointState = WaypointState.Passed; // will be ignored on the next cycle
Talker.Say(10, "Waypoint " + nextWp.number + " passed");
}
else if (nextWp.estimatedTimeOfArrival.HasValue && (DateTime.Now - nextWp.estimatedTimeOfArrival.Value).TotalSeconds > WAYPOINT_CANTREACH_SECONDS)
{
nextWp.waypointState = WaypointState.CouldNotReach; // will be ignored on the next cycle
Talker.Say(10, "Waypoint " + nextWp.number + " could not reach");
}
else
{
setCurrentGoalBearingRelativeToRobot(dirToWp.bearingRelative.Value);
setCurrentGoalDistance(distToWp.Meters);
FollowDirectionMaxVelocityMmSec = Math.Min(distToWp.Meters > 3.0d ? MaximumForwardVelocityMmSec : ModerateForwardVelocityMmSec,
_state.collisionState.canMoveForwardSpeedMms); // mm/sec
Tracer.Trace(string.Format("IP: distToWp.Meters= {0}, FollowDirectionMaxVelocityMmSec={1}", distToWp.Meters, FollowDirectionMaxVelocityMmSec));
if (!nextWp.estimatedTimeOfArrival.HasValue && FollowDirectionMaxVelocityMmSec != 0.0d)
{
nextWp.waypointState = WaypointState.SelectedAsTarget;
double timeToReachSec = (distToWp.Meters * 1000.0d) / (FollowDirectionMaxVelocityMmSec); // mm/sec
nextWp.estimatedTimeOfArrival = DateTime.Now.AddSeconds(timeToReachSec);
}
// choose current tactics:
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.FollowDirection;
}
}
else
{
Talker.Say(10, "Last Waypoint " + nextWp.number + " passed, stopping");
// choose current tactics - we are done, stop:
_mapperVicinity.robotState.robotTacticsType = RobotTacticsType.None;
_mapperVicinity.robotDirection.bearing = null;
}
}
private void drawMission(List <UIElement> relList, List <UIElement> unrotatedList)
{
if (_currentRoutePlanner != null && _currentRoutePlanner.mission != null && _currentRoutePlanner.mission.waypoints.Any())
{
try
{
double frameWidth = canvasRel.ActualWidth;
double frameHeight = canvasRel.ActualHeight;
double metersPerPixelX = MapperSettings.elementSizeMeters / (frameWidth / mapperVicinity.RelMapWidth);
double metersPerPixelY = MapperSettings.elementSizeMeters / (frameHeight / mapperVicinity.RelMapHeight);
double latitudeFactor = Math.Cos(mapperVicinity.robotPosition.Y);
double xMeters = Distance.METERS_PER_DEGREE * latitudeFactor;
int i = 0;
LocationWp prevWp = null;
double xPixPrev = 0, yPixPrev = 0;
foreach (LocationWp curWp in _currentRoutePlanner.mission.waypoints)
{
Color wpColor;
switch (curWp.waypointState)
{
default:
case WaypointState.None:
wpColor = Colors.Blue;
break;
case WaypointState.SelectedAsTarget:
wpColor = Colors.Orange;
break;
case WaypointState.Passed:
wpColor = Colors.Green;
break;
case WaypointState.CouldNotReach:
wpColor = Colors.Red;
break;
}
DetectedObjectViewControl dovc = new DetectedObjectViewControl()
{
color = wpColor, size = curWp.isHome ? 121 : 41
};
Direction dirToWp = curWp.directionToWp(mapperVicinity.robotPosition, mapperVicinity.robotDirection);
Distance distToWp = curWp.distanceToWp(mapperVicinity.robotPosition);
double metersToWaypoint = Math.Round(distToWp.Meters, 1);
distToWp /= 10.0d; // fit to small scale vicinity map
RelPosition pos = new RelPosition(dirToWp, distToWp);
double distXmeters = pos.X;
double distYmeters = pos.Y;
double xPix = frameWidth / 2.0d + distXmeters / metersPerPixelX;
double yPix = frameHeight / 2.0d - distYmeters / metersPerPixelY;
dovc.Margin = new Thickness(xPix, yPix, 0, 0);
relList.Add(dovc);
string wptCommand = curWp.id == MAV_CMD.WAYPOINT ? (curWp.isHome ? "HOME" : string.Empty) : curWp.id.ToString();
string strSecToWp = string.Empty;
if (curWp.waypointState == WaypointState.SelectedAsTarget && curWp.estimatedTimeOfArrival.HasValue)
{
strSecToWp = string.Format(" {0:#} sec", (curWp.estimatedTimeOfArrival.Value - DateTime.Now).TotalSeconds);
}
Label labelWp = new Label()
{
Content = string.Format("WPT{0}: {1} m {2} deg {3}{4}", curWp.number, metersToWaypoint, Math.Round((double)dirToWp.bearingRelative, 1), wptCommand, strSecToWp),
Margin = new Thickness(xPix + 10, yPix + 10, 0, 0),
FontSize = 14,
FontWeight = FontWeights.Bold
};
relList.Add(labelWp);
if (prevWp != null)
{
//Line lineToWp = new Line() { X1 = xPixPrev, Y1 = yPixPrev, X2 = xPix, Y2 = yPix, Stroke=Brushes.Red, StrokeThickness = 4.0d, StrokeEndLineCap = PenLineCap.Triangle };
//relList.Add(lineToWp);
Brush brush = Brushes.Blue;
double thickness = 4.0d;
if (prevWp.waypointState == WaypointState.Passed && curWp.waypointState == WaypointState.Passed)
{
// leg has been traveled:
brush = Brushes.Green;
}
else if (curWp.waypointState == WaypointState.SelectedAsTarget)
{
// current leg:
brush = Brushes.OrangeRed;
thickness = 6.0d;
}
relList.Add(LineArrowHelper.DrawLinkArrow(new Point(xPixPrev, yPixPrev), new Point(xPix, yPix), brush, thickness));
}
prevWp = curWp;
xPixPrev = xPix;
yPixPrev = yPix;
i++;
}
}
catch { }
}
}