/// <summary>
/// Update rover position and remove nodes from waypoints and planned path if the rover approaches them
/// </summary>
/// <param name="pose"></param>
public void UpdatePose(RobotPose pose)
{
lock (plannerLock)
{
List<SimpleTreeNode<Vector2>> nodesToRemove = new List<SimpleTreeNode<Vector2>>();
currentLocation = pose.ToVector2();
//Remove waypoints from the list as the rover nears them
nodesToRemove.Clear();
foreach (SimpleTreeNode<Vector2> node in userWaypoints)
{
if (currentLocation.DistanceTo(node.Value) < wayPointRadius)
{
nodesToRemove.Add(node);
}
else break;
}
foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
{
userWaypoints.Remove(node);
}
//Remove nodes from the planned path as the rover approaches them
lock (pathLock)
{
nodesToRemove.Clear();
foreach (SimpleTreeNode<Vector2> node in outputNodeList)
{
if (currentLocation.DistanceTo(node.Value) < pathPointRadius)
{
nodesToRemove.Add(node);
}
else break;
}
foreach (SimpleTreeNode<Vector2> node in nodesToRemove)
{
outputNodeList.Remove(node);
}
}
}
}
public void UpdatePose(RobotPose pose)
{
this.pose = pose;
if (waypoints == null || waypoints.Count == 0) return;
if (waypointsAchieved >= waypoints.Count) return;
if (pose.ToVector2().DistanceTo(waypoints.ElementAt(waypointsAchieved).Coordinate) < achieveThreshold)
{
waypoints.ElementAt(waypointsAchieved).Achieved = true;
WaypointAchieved(this, new WaypointAchievedEventArgs(waypoints[waypointsAchieved]));
waypointsAchieved++;
}
}