//****************** 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)); }