public DetectedObjectBase(Direction dir, Distance dist)
: this()
{
relPosition = new RelPosition(dir, dist);
}
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 { }
}
}