private void drawDetectedObjects(List <UIElement> relList, List <UIElement> unrotatedList, int method)
{
try
{
MapperVicinity mapperVicinity = CurrentMapper;
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;
switch (method)
{
case 0:
default:
i = mapperVicinity.Count;
break;
case 1:
lock (mapperVicinity)
{
foreach (IDetectedObject idobj in mapperVicinity)
{
DetectedObjectViewControl dovc = null;
if (idobj.objectType == DetectedObjectType.Mark)
{
dovc = new DetectedObjectViewControl()
{
color = idobj.color, size = 21
};
}
else
{
dovc = new DetectedObjectViewControl()
{
color = idobj.color
};
}
/*
* GeoPosition pos = idobj.geoPosition;
*
* double distXmeters = (pos.Lng - mapperVicinity.robotPosition.Lng) * Distance.METERS_PER_DEGREE * latitudeFactor;
* double distYmeters = (pos.Lat - mapperVicinity.robotPosition.Lat) * Distance.METERS_PER_DEGREE;
*/
RelPosition pos = idobj.relPosition;
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);
i++;
}
}
break;
}
// draw number of Detected Objects:
double x0 = 20.0d;
double y0 = 20.0d;
Label lN = new Label()
{
Content = string.Format("{0} objects", i), Margin = new Thickness(x0, y0, 0, 0), FontSize = 10, FontWeight = FontWeights.Normal
};
unrotatedList.Add(lN);
}
catch { }
}
private void drawDetectedObjects(List<UIElement> relList, List<UIElement> unrotatedList, int method)
{
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;
switch (method)
{
case 0:
default:
i = mapperVicinity.Count;
break;
case 1:
lock (mapperVicinity)
{
foreach (IDetectedObject idobj in mapperVicinity)
{
DetectedObjectViewControl dovc = null;
if (idobj.objectType == DetectedObjectType.Mark)
{
dovc = new DetectedObjectViewControl() { color = idobj.color, size = 21 };
}
else
{
dovc = new DetectedObjectViewControl() { color = idobj.color };
}
/*
GeoPosition pos = idobj.geoPosition;
double distXmeters = (pos.Lng - mapperVicinity.robotPosition.Lng) * Distance.METERS_PER_DEGREE * latitudeFactor;
double distYmeters = (pos.Lat - mapperVicinity.robotPosition.Lat) * Distance.METERS_PER_DEGREE;
*/
RelPosition pos = idobj.relPosition;
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);
i++;
}
}
break;
}
// draw number of Detected Objects:
double x0 = 20.0d;
double y0 = 20.0d;
Label lN = new Label() { Content = string.Format("{0} objects", i), Margin = new Thickness(x0, y0, 0, 0), FontSize = 10, FontWeight = FontWeights.Normal };
unrotatedList.Add(lN);
}
catch { }
}
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 { }
}
}
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 { }
}
}
