/******************************/
// Waypoint Flight Logic //
/******************************/
/// <summary>
/// To be called by ROSConnection when waypoints have been successfully uploaded.
/// Inform each waypoint that it has been uploaded for state change and user feedback.
/// </summary>
public void WaypointsUploaded(MissionWaypointMsg[] uploadedWaypoints)
{
foreach (MissionWaypointMsg waypoint in uploadedWaypoints)
{
Vector3 waypoint_coord = WorldProperties.GPSCoordToUnityCoord(new GPSCoordinate(waypoint.GetLatitude(), waypoint.GetLongitude(), waypoint.GetAltitude()));
// TODO: refine search to accound for order.
for (int i = 1; i < waypoints.Count; i++)
{
Waypoint unityWaypoint = waypoints[i];
float distance = Vector3.Distance(unityWaypoint.gameObjectPointer.transform.localPosition, waypoint_coord);
if (distance < 0.2f)
{
unityWaypoint.waypointProperties.WaypointUploaded();
}
}
}
}
