//****************** Consturtors ******************
public Navigator(IObstacleMap obstacleMap, IObstacleDetector obstacleDetector, ObstacleLocalizer obstacleLocalizer,
ICartographer cartographer, IDoorDetector doorDetector, Pose pose, SonarDetector sonarDetector,
SensorArray sensorArray)
{
this.ObstacleDetector = obstacleDetector;
this.ObstacleMap = obstacleMap;
this.ObstacleLocalizer = obstacleLocalizer;
this.DoorDetector = doorDetector;
this.CurrentPose = pose;
this.Cartographer = cartographer;
this.SonarDetector = sonarDetector;
this.ChairSensorArray = sensorArray;
pose.CurrentRoom = Cartographer.GetStartingRoom();
LastPoint = pose.CurrentPositionInUV;
FollowRouteThread = new Thread(UpdateRoute);
FollowRouteThread.Name = "FollowRouteThread";
FollowRouteThread.Start();
this.ObstacleDetector.ObstaclesDetected += ObstacleDetector_ObstaclesDetected; // ?
}
//returns a list of mPoints in UV that we can use to navigate a room.
//if they are the same point it will just return that point
//note: this does include the start point.
//if this fails it will return null
public List <mPoint> SearchPath(mPoint startPointUV, mPoint endPointUV, IObstacleMap obstacleMap)
{
List <mPoint> returnList = new List <mPoint>();
MapRoom room1 = GetRoomFromPointInUV(startPointUV);
if (room1 == null)
{
return(null);
}
MapRoom room2 = GetRoomFromPointInUV(endPointUV);
if (room2 == null)
{
return(null);
}
//The names of each room are unique this is enforced when the map is created
if (room1.Name != room2.Name)
{
return(null);
}
if (startPointUV == endPointUV)
{
returnList.Add(startPointUV);
return(returnList);
}
mGridPoint startingGridPoint = room1.GetClosestGridPoint(startPointUV);
if (startingGridPoint == null)
{
return(null);
}
mGridPoint endingGridPoint = room1.GetClosestGridPoint(endPointUV);
if (startingGridPoint == null)
{
return(null);
}
PathPQueue queue = new PathPQueue();
//hashset to keep track of the visited points
HashSet <mPoint> tableOfVisitedPoints = new HashSet <mPoint>();
Console.Out.WriteLine(room1.ToString());
//set up the queue and cast the starting point in UV to a grid point so we can store it in the
//PathSearchNode.
List <mGridPoint> startingListOfGPoints = new List <mGridPoint>();
startingListOfGPoints.Add(new mGridPoint(startPointUV.X, startPointUV.Y, room1));
startingListOfGPoints.Add(startingGridPoint);
queue.Add(new PathSearchNode(startingGridPoint, startingListOfGPoints, 0));
while (!queue.IsEmpty())
{
PathSearchNode currentNode = queue.Pop();
tableOfVisitedPoints.Add(currentNode.CurrentGridPoint);
if (currentNode.CurrentGridPoint == endingGridPoint)
{
//add the last point to the list
returnList = currentNode.GetListOfMPoints();
returnList.Add(endPointUV);
printPathSearch(currentNode, queue, tableOfVisitedPoints, room1, startingGridPoint, endingGridPoint, currentNode.CurrentGridPoint, new List <mGridPoint>());
return(returnList);
}
List <mGridPoint> adjacentGridPoints = room1.GetAdjacentGridPoints(currentNode.CurrentGridPoint);
foreach (mGridPoint adjGPoint in adjacentGridPoints)
{
//if we have not visited this point and it is passible
bool isPointObstructed = false; // obstacleMap.IsPointObstructed(adjGPoint, room1);
if (!tableOfVisitedPoints.Contains(adjGPoint) && adjGPoint.IsPassible() && !isPointObstructed)
{
double newDistance = room1.GetWeightedDistance(currentNode.CurrentGridPoint, adjGPoint, BufferDistance);
//get weighted distance returns -1 if it crosses an object.
if (newDistance > 0)
{
//add the point to the queue make a deep copy
List <mGridPoint> tmpGridPointList = new List <mGridPoint>(currentNode.ListOfGridPoints);
tmpGridPointList.Add(adjGPoint);
queue.Add(new PathSearchNode(adjGPoint, tmpGridPointList, currentNode.Distance + newDistance));
}
}
}
}
return(null);
}
//takes two points one for the currentLocation and one for the desired endLocation. It calls the room
//search algorithm to find the best path to a point in a room.
public List <mPoint> PlanPath(mPoint currentLocationInUV, mPoint endLocation, IObstacleMap obstacleMap)
{
return(BuildingMap.SearchPath(currentLocationInUV, endLocation, obstacleMap));
}
