//Find the shortest Reeds-Shepp path
        List <Node> GetShortestReedsSheppPath(Node nextNode, Transform goalTrans, CarData carData)
        {
            //Get the current position and heading of the car
            //In Hybrid A* we can't use the car, but the current position and heading in search tree
            Vector3 treeCarPos = nextNode.carPos;
            //We store heading in radians but dubins generator is currently using degrees as input
            float startHeading = nextNode.heading;

            Vector3 goalPos = goalTrans.position;

            float goalHeading = goalTrans.eulerAngles.y * Mathf.Deg2Rad;

            List <Node> shortestPath = reedsSheppPathGenerator.GetShortestReedSheppPath(
                treeCarPos,
                startHeading,
                goalPos,
                goalHeading);

            //If we have a path and it is not blocked by obstacle
            //if (shortestPath != null && ObstaclesDetection.IsFixedPathDrivable(shortestPath, carData)) (MATT)
            if (shortestPath != null)

            {
                return(shortestPath);
            }

            return(null);
        }
        // Use this for initialization
        void Start()
        {
            carData        = GetComponent <CarData>();
            targetPosition = new Vector3(195, 0, 25);

            //Draw the lidar visualization
            line = gameObject.GetComponent <LineRenderer>();
            line.SetVertexCount(numLineSegments + 1);
            line.useWorldSpace = false;
            DrawLidar();
        }
        void Awake()
        {
            egoCar          = GameObject.Find("EgoCar");
            egoCarInterface = egoCar.GetComponent <EgoCarInterface>();
            egoCarData      = egoCar.GetComponent <CarData>();

            heuristicsController = new HeuristicsController();

            smoothPathController = new SmoothPathController();

            debugController = GetComponent <DebugController>();
        }
        //Check if the target car has a valid position
        private bool HasTargetCarValidPosition(Vector3 targetPos, float heading, CarData carData)
        {
            bool  hasValidPosition = false;
            float targetCarHeading = heading * Mathf.Deg2Rad;

            if (ObstaclesDetection.TargetPositionWithinTrack(targetPos, targetCarHeading, carData)) //(MATT)
            {
                hasValidPosition = true;
            }

            return(hasValidPosition);
        }
        void Start()
        {
            //Move the center of mass down
            Rigidbody carRB = GetComponent <Rigidbody>();

            carRB.centerOfMass = carRB.centerOfMass - new Vector3(0f, 0.8f, 0f);

            PIDScript = GetComponent <PIDController>();

            carData = GetComponent <CarData>();

            followPathScript = GetComponent <FollowPath>();
        }
        //Test if one path is drivable
        public static bool IsFixedPathDrivable(List <Node> path, CarData carData)
        {
            for (int i = 0; i < path.Count; i++)
            {
                Vector3 carPos = path[i].carPos;

                float carHeading = path[i].heading;

                if (HasCarInvalidPosition(carPos, carHeading, carData))
                {
                    //This path is not drivable
                    return(false);
                }
            }

            //This path is drivable
            return(true);
        }
Example #7
0
        //Generate a path with Hybrid A*
        public void GenerateHybridAStarPath(Transform targetTrans, List <Node> finalPath, List <Node> allExpandedNodes)
        {
            CarData targetCarData = targetTrans.GetComponent <CarData>();

            //Get the data belonging to the current active car
            this.carData = SimController.current.GetActiveCarData();
            //Each car may have a different turning radius
            reedsSheppPathGenerator = new GenerateReedsShepp(carData.GetTurningRadius());

            //Everything we need to reset before we begin
            Reset();

            //Run the main loop
            RunHybridAStar(targetCarData, allExpandedNodes);

            //Generate the final path when Hybrid A* has found the goal
            GenerateFinalPath(finalPath);
        }
        void Start()
        {
            //for testing
            InvokeRepeating("ResetObstacles", 0f, 10f);
            //Generate obstacles
            //Has to do it from this script or the obstacles might be created after this script has
            //finished and mess things up
            GetComponent <ObstaclesController>().InitObstacles();

            hybridAStar      = new HybridAStar();
            hybridAStarAngle = new HybridAStarAngle();


            //Create the textures showing various stuff
            debugController.DisplayCellObstacleIntersection();
            debugController.DisplayObstacleFlowField();

            targetCarTrans.position = egoCarData.GetRearWheelPos();
            targetCarTrans.rotation = egoCarData.GetCarTransform().rotation;
            targetCarData           = targetCarTrans.GetComponent <CarData>();

            Debug.Log(string.Format("Target car position: {0}", targetCarData.GetCarTransform().position));
            Debug.Log(string.Format("Target car rear wheel: {0}", targetCarData.GetRearWheelPos()));
        }
        //Run the main loop
        private void RunHybridAStar(List <Node> allExpandedNodes, CarData targetCarData)
        {
            //Why rear wheel? Because we need that position when simulating the "skeleton" car
            //and then it's easier if everything is done from the rear wheel positions
            Vector3 startPos = carData.GetRearWheelPos();

            IntVector2 startCellPos = PathfindingController.ConvertCoordinateToCellPos(startPos);

            lowestCostForward[startCellPos.x, startCellPos.z] = 0f;
            lowestCostReverse[startCellPos.x, startCellPos.z] = 0f;

            //Create a new node
            Node node = new Node();

            //Add the initial car data to the start node
            node.g             = 0f;
            node.h             = HeuristicsController.heuristics[startCellPos.x, startCellPos.z];
            node.cellPos       = startCellPos;
            node.carPos        = startPos;
            node.heading       = carData.GetHeading() * Mathf.Deg2Rad;
            node.steeringAngle = 0f;
            node.isReversing   = false;

            openNodes.Add(node);

            //Init the bad node
            this.badNode = node;

            //Bools so we can break out of the main loop
            //Set when search is complete
            bool found = false;
            //Set if we can't find a node to expand
            bool resign = false;
            //To identify the best of the bad nodes
            //bestDistance = Mathf.Infinity;
            //To break out of the loop if it takes too long time
            int iterations = 0;

            while (!found && !resign)
            {
                if (iterations > 100000)
                {
                    Debug.Log("Stuck in infinite loop");

                    break;
                }

                iterations += 1;

                //If we don't have any nodes to expand
                if (openNodes.Count == 0)
                {
                    resign = true;

                    Debug.Log("Failed to find a path");
                }
                //We have nodes to expand
                else
                {
                    //Get the node with the lowest cost
                    Node nextNode = openNodes.RemoveFirst();

                    //Save it in case we can find an entire path if it has a lower cost
                    //Use heuristics to determine if this node is close to the goal than a previous node
                    if (nextNode.h < badNode.h)
                    {
                        this.badNode = nextNode;
                    }


                    //Close this cell
                    IntVector2 cellPos = nextNode.cellPos;

                    if (nextNode.isReversing)
                    {
                        closedCellsReverse[cellPos.x, cellPos.z] = true;
                    }
                    else
                    {
                        closedCellsForward[cellPos.x, cellPos.z] = true;
                    }



                    //Check if this is a goal node
                    //Use an accuracy of 1 m because we will not hit the exact target coordinate
                    float distanceSqrToGoal = (nextNode.carPos - targetCarData.GetRearWheelPos()).sqrMagnitude;

                    //But we also need to make sure the car has correct heading
                    float headingDifference = Mathf.Abs(targetCarData.GetHeading() - nextNode.heading * Mathf.Rad2Deg);

                    if (distanceSqrToGoal < 1f && headingDifference < 20f)
                    {
                        found = true;

                        Debug.Log("Found a path");

                        finalNode = nextNode;

                        //Make sure the end node has the same position as the target
                        finalNode.carPos.x = targetCarData.GetRearWheelPos().x;
                        finalNode.carPos.z = targetCarData.GetRearWheelPos().z;
                    }
                    //If we havent found the goal, then expand this node
                    else
                    {
                        float distSqr = (nextNode.carPos - targetCarData.GetRearWheelPos()).sqrMagnitude;

                        //Test if we can find the goal with a fixed path algorithm such as Dubins or Reeds-Shepp
                        List <Node> fixedPath = null;

                        //Don't try to find a fixed path each expansion, but try to find more fixed paths the close to the goal we are
                        if (
                            (allExpandedNodes.Count % 300 == 0) ||
                            (allExpandedNodes.Count % 20 == 0 && distSqr < 40f * 40f) ||
                            (distSqr < 20f * 20f))
                        {
                            fixedPath = GetShortestReedsSheppPath(nextNode, targetCarData.GetCarTransform(), carData);
                        }

                        //If a fixed path is possible
                        if (fixedPath != null)
                        {
                            //Stop looping - real Hybrid A* continues looping and just add this node as a node in the tree
                            found = true;

                            Debug.Log("Found a path with a fixed path algorithm");

                            //Generate nodes along this path until we reach the goal
                            Node previousNode = nextNode;

                            //Don't need the first coordinate because it is the same as the position from the tree (nextNode)
                            for (int i = 1; i < fixedPath.Count; i++)
                            {
                                fixedPath[i].previousNode = previousNode;

                                previousNode = fixedPath[i];
                            }

                            finalNode = previousNode;

                            //Make sure the end node has the same position as the target
                            finalNode.carPos.x = targetCarData.GetRearWheelPos().x;
                            finalNode.carPos.z = targetCarData.GetRearWheelPos().z;
                        }
                        else
                        {
                            ExpandNode(nextNode);

                            //For debugging
                            allExpandedNodes.Add(nextNode);
                        }
                    }
                }
            }
        }
        //Check if the car is colliding with an obstacle or is outside of map
        public static bool HasCarInvalidPosition(Vector3 carRearWheelPos, float heading, CarData carData)
        {
            bool hasInvalidPosition = false;

            //Make the car bigger than it is to be on the safe side
            float marginOfSafety = 0.5f;

            float carLength = carData.GetLength() + marginOfSafety;
            float carWidth  = carData.GetWidth() + marginOfSafety;


            //Find the center pos of the car (carPos is at the rearWheels)
            float distCenterToRearWheels = carData.GetDistanceToRearWheels();

            float xCenter = carRearWheelPos.x + distCenterToRearWheels * Mathf.Sin(heading);
            float zCenter = carRearWheelPos.z + distCenterToRearWheels * Mathf.Cos(heading);

            Vector3 carPos = new Vector3(xCenter, carRearWheelPos.y, zCenter);


            //
            // Step 1. Check if the car's corners is inside of the map
            //

            //Find all corners of the car
            Rectangle carCornerPos = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);

            //Detect if any of the corners is outside of the map
            if (
                !PathfindingController.IsPositionWithinGrid(carCornerPos.FL) ||
                !PathfindingController.IsPositionWithinGrid(carCornerPos.FR) ||
                !PathfindingController.IsPositionWithinGrid(carCornerPos.BL) ||
                !PathfindingController.IsPositionWithinGrid(carCornerPos.BR))
            {
                //At least one of the corners is outside of the map
                hasInvalidPosition = true;

                return(hasInvalidPosition);
            }



            //
            // Step 2. Check if the car's center position is far away from an obstacle
            //
            //We dont need to check if the car is colliding with an obstacle if the distance to an obstacle is great
            IntVector2 cellPos = PathfindingController.ConvertCoordinateToCellPos(carPos);

            //The car is not colliding with anything if the steps to an obstacle is greater than the length of the car
            if (ObstaclesController.distanceToClosestObstacle[cellPos.x, cellPos.z] > carData.GetLength())
            {
                //This is a valid position
                hasInvalidPosition = false;

                return(hasInvalidPosition);
            }



            //
            // Step 3. Check if the car is hitting an obstacle
            //

            //Find all corners of the car
            Rectangle carCornerPosFat = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);

            //Method 1 - Use the car's corners and then rectangle-rectangle-intersection with the obstacles
            hasInvalidPosition = ObstacleDetectionCorners(carPos, carCornerPosFat);

            //Method 2 - Approximate the car with circles
            //hasInvalidPosition = ObstacleDetectionCircles(carCenterPos, heading, carData, carCornerPosFat);


            return(hasInvalidPosition);
        }
        //Approximate the car's area with circles to detect if there's an obstacle within the circle
        private static bool ObstacleDetectionCircles(Vector3 carPos, float heading, CarData carData, Rectangle carCornerPos)
        {
            bool hasInvalidPosition = false;

            Vector3[] circlePositions = new Vector3[3];

            //Get the position of the 3 circles that approximates the size of the car
            circlePositions = SkeletonCar.GetCirclePositions(carPos, heading, circlePositions);

            //
            //Find all obstacles close to the car
            //
            //The car's length is 5 m and each obstacle is 1 m, so add a little to be on the safe side
            //float searchRadius = carData.GetLength() * 0.5f + 1.5f;

            //List<ObstacleData> obstaclesThatAreClose = FindCloseObstaclesWithinRadius(carPos, searchRadius * searchRadius);

            List <ObstacleData> obstaclesThatAreClose = FindCloseObstaclesCell(carPos, carCornerPos);

            //If there are no obstacle close, then return
            if (obstaclesThatAreClose.Count == 0)
            {
                return(hasInvalidPosition);
            }


            //
            //If there are obstacles around the car, then we have to see if some of them intersect
            //
            //The radius of one circle that approximates the area of the car
            //The width of the car is 2 m but the circle has to be larger to provide a margin of safety
            float circleRadius = 1.40f;

            //But we also need to add the radius of the box obstacle which has a width of 1 m
            float criticalRadius = circleRadius + 0.5f;

            //And square it to speed up
            float criticalRadiusSqr = criticalRadius * criticalRadius;

            //Loop through all circles and detect if there's an obstacle within the circle
            for (int i = 0; i < circlePositions.Length; i++)
            {
                Vector3 currentCirclePos = circlePositions[i];

                //Is there an obstacle within the radius of this circle
                for (int j = 0; j < obstaclesThatAreClose.Count; j++)
                {
                    float distSqr = (currentCirclePos - obstaclesThatAreClose[j].centerPos).sqrMagnitude;

                    if (distSqr < criticalRadiusSqr)
                    {
                        hasInvalidPosition = true;

                        return(hasInvalidPosition);
                    }
                }
            }


            return(hasInvalidPosition);
        }
        //Check if the car outside of map (MATT)
        public static bool TargetPositionWithinTrack(Vector3 carRearWheelPos, float heading, CarData carData)
        {
            bool withinTrack = true;

            //Make the car bigger than it is to be on the safe side
            float marginOfSafety = 0.5f;

            float carLength = carData.GetLength() + marginOfSafety;
            float carWidth  = carData.GetWidth() + marginOfSafety;


            //Find the center pos of the car (carPos is at the rearWheels)
            float distCenterToRearWheels = carData.GetDistanceToRearWheels();

            float xCenter = carRearWheelPos.x + distCenterToRearWheels * Mathf.Sin(heading);
            float zCenter = carRearWheelPos.z + distCenterToRearWheels * Mathf.Cos(heading);

            Vector3 carPos = new Vector3(xCenter, carRearWheelPos.y, zCenter);

            //Find all corners of the car
            Rectangle carCornerPos = SkeletonCar.GetCornerPositions(carPos, heading, carWidth, carLength);

            //Detect if any of the corners is outside of the map
            if (
                !PathfindingController.IsPositionWithinGrid(carCornerPos.FL) ||
                !PathfindingController.IsPositionWithinGrid(carCornerPos.FR) ||
                !PathfindingController.IsPositionWithinGrid(carCornerPos.BL) ||
                !PathfindingController.IsPositionWithinGrid(carCornerPos.BR))
            {
                //At least one of the corners is outside of the map
                withinTrack = false;
            }

            return(withinTrack);
        }
Example #13
0
        //Run the main loop
        private void RunHybridAStar(CarData targetCarData, List <Node> allExpandedNodes)
        {
            //Why rear wheel? Because we need that position when simulating the "skeleton" car
            //and then it's easier if everything is done from the rear wheel positions
            Vector3 startPos = carData.GetRearWheelPos();

            IntVector2 startCellPos = PathfindingController.ConvertCoordinateToCellPos(startPos);

            //Create a new node
            Node node = new Node();

            //Add the initial car data to the start node
            node.g             = 0f;
            node.h             = HeuristicsController.heuristics[startCellPos.x, startCellPos.z];
            node.cellPos       = startCellPos;
            node.carPos        = startPos;
            node.heading       = carData.GetHeading() * Mathf.Deg2Rad;
            node.steeringAngle = 0f;
            node.isReversing   = false;

            openNodes.Add(node);

            //Init the bad node
            this.badNode = node;

            //Bools so we can break out of the main loop
            //Set when search is complete
            bool found = false;
            //Set if we can't find a node to expand
            bool resign = false;
            //To identify the best of the bad nodes
            //bestDistance = Mathf.Infinity;
            //To break out of the loop if it takes too long time
            int iterations = 0;

            while (!found && !resign)
            {
                if (iterations > 100000)
                {
                    Debug.Log("Stuck in infinite loop");

                    break;
                }

                iterations += 1;

                //If we don't have any nodes to expand
                if (openNodes.Count == 0)
                {
                    resign = true;

                    Debug.Log("Failed to find a path");
                }
                //We have nodes to expand
                else
                {
                    //Get the node with the lowest cost
                    Node nextNode = openNodes.RemoveFirst();

                    //Save it in case we can find an entire path if it has a lower cost
                    //Use heuristics to determine if this node is vlose to the goal than a previous node
                    if (nextNode.h < badNode.h)
                    {
                        this.badNode = nextNode;
                    }


                    //Close this cell
                    IntVector2 cellPos = nextNode.cellPos;

                    int roundedAngle = RoundAngle(nextNode.heading * Mathf.Rad2Deg);

                    Dictionary <int, bool> currentAngles = closedCells[cellPos.x, cellPos.z];

                    //Close the cell with this angle
                    if (!currentAngles.ContainsKey(roundedAngle))
                    {
                        currentAngles.Add(roundedAngle, true);
                    }
                    else
                    {
                        //This is not costly so it souldnt be counted as an iteration
                        //Is needed because we are not removing nodes with higher cost but the same angle from the heap
                        iterations -= 1;

                        continue;
                    }



                    //Check if this is a goal node
                    //Use an accuracy of 1 m because we will not hit the exact target coordinate
                    float distanceSqrToGoal = (nextNode.carPos - targetCarData.GetRearWheelPos()).sqrMagnitude;

                    //But we also need to make sure the car has correct heading
                    float headingDifference = Mathf.Abs(targetCarData.GetHeading() - nextNode.heading * Mathf.Rad2Deg);

                    if (distanceSqrToGoal < 1f && headingDifference < 20f)
                    {
                        found = true;

                        Debug.Log("Found a path");

                        finalNode = nextNode;

                        //Make sure the end node has the same position as the target
                        finalNode.carPos.x = targetCarData.GetRearWheelPos().x;
                        finalNode.carPos.z = targetCarData.GetRearWheelPos().z;
                    }
                    //If we havent found the goal, then expand this node
                    else
                    {
                        //Test if we can find the goal with a fixed path algorithm such as Dubins or Reeds-Shepp
                        List <Node> fixedPath = null;

                        //Don't try to find a fixed path each expansion, but try to find more fixed paths the close to the goal we are
                        if (
                            (allExpandedNodes.Count % 300 == 0) ||
                            (allExpandedNodes.Count % 20 == 0 && distanceSqrToGoal < 40f * 40f)
                            )
                        {
                            fixedPath = GetShortestReedsSheppPath(nextNode, targetCarData.GetCarTransform(), carData);

                            //If a fixed path is possible
                            if (fixedPath != null)
                            {
                                //Add this node to the open list
                                //Not 0 because that's the node we are expanding from
                                Node fixedPathNode = fixedPath[1];

                                fixedPathNode.cellPos      = PathfindingController.ConvertCoordinateToCellPos(fixedPathNode.carPos);
                                fixedPathNode.h            = HeuristicsController.heuristics[fixedPathNode.cellPos.x, fixedPathNode.cellPos.z];
                                fixedPathNode.previousNode = nextNode;
                                //Add the other car data to the node
                                //This is not exactly true but almost true because this node does almost have the same steering angle as the last node
                                fixedPathNode.steeringAngle = 0f;

                                //Now we can calculate the cost to reach this node
                                fixedPathNode.g = CalculateCosts(fixedPathNode);

                                //Add the node to the list with open nodes
                                openNodes.Add(fixedPathNode);
                            }
                        }


                        ExpandNode(nextNode);

                        //For debugging
                        allExpandedNodes.Add(nextNode);
                    }
                }
            }
        }
        void Start()
        {
            carScript = GetComponent <CarController>();

            carData = GetComponent <CarData>();
        }