コード例 #1
0
        public virtual void ReplanGlobal(Platform platform)
        {
            // create searchGrid data structure for the EpPathFinding
            BaseGrid searchGrid = new StaticGrid(platform.Map.Rows, platform.Map.Columns);
            List <Tuple <double, GridPos> > searchPoses = new List <Tuple <double, GridPos> > (); // possible search poses

            for (int i = 0; i < platform.Map.Rows; i++)
            {
                for (int j = 0; j < platform.Map.Columns; j++)
                {
                    if (platform.Map.MapMatrix[i, j] < platform.OccupiedThreshold)
                    {
                        searchGrid.SetWalkableAt(i, j, true);
                    }

                    if ((platform.Map.MapMatrix[i, j] >= platform.FreeThreshold) && (platform.Map.MapMatrix[i, j] <= platform.OccupiedThreshold))
                    {
                        RegionLimits limits = platform.Map.CalculateLimits(i, j, 1);
                        List <Pose>  posesl = limits.GetPosesWithinLimits();
                        foreach (Pose p in posesl)
                        {
                            if (platform.Map.MapMatrix[p.X, p.Y] < platform.FreeThreshold)
                            {
                                double d = Math.Sqrt(Math.Pow(p.X - platform.Pose.X, 2) + Math.Pow(p.Y - platform.Pose.Y, 2));
                                searchPoses.Add(new Tuple <double, GridPos>(d, new GridPos(i, j)));
                                break;
                            }
                        }
                    }
                }
            }

            // set unaccessable for those places where are platforms and their enviroment within 1 step radius
            foreach (Platform plt in platform.ObservedPlatforms)
            {
                if (plt.Equals(platform))
                {
                    continue;
                }
                RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
                List <Pose>  posesl = limits.GetPosesWithinLimits();
                foreach (Pose p in posesl)
                {
                    searchGrid.SetWalkableAt(p.X, p.Y, false);
                }
            }

            // bound the search to avoid large computation
            // select the first closest 50 candidates based on L2 distance
            int maxNumOfSearchPoses = 50;

            searchPoses.Sort((t1, t2) => t1.Item1.CompareTo(t2.Item1));
            if (searchPoses.Count > maxNumOfSearchPoses)
            {
                searchPoses.RemoveRange(maxNumOfSearchPoses, searchPoses.Count - maxNumOfSearchPoses);
            }

            // init search
            GridPos        startPos = new GridPos(platform.Pose.X, platform.Pose.Y);
            GridPos        endPos   = new GridPos(20, 10);
            JumpPointParam jpParam  = new JumpPointParam(searchGrid, startPos, endPos, false, true, true);

            // find the best path
            double         bestPathScore = Double.PositiveInfinity;
            List <GridPos> bestPath      = null;

            foreach (Tuple <double, GridPos> p in searchPoses)
            {
                //jpParam.Reset(startPos, p);
                jpParam.Reset(new GridPos(platform.Pose.X, platform.Pose.Y), p.Item2);
                List <GridPos> resultPathList = JumpPointFinder.FindPath(jpParam);

                if (resultPathList.Count > 2)
                {
                    double score = 0;
                    for (int i = 1; i < resultPathList.Count; i++)
                    {
                        score += Math.Sqrt(Math.Pow(resultPathList[i].x - resultPathList[i - 1].x, 2) + Math.Pow(resultPathList[i].y - resultPathList[i - 1].y, 2));
                    }

                    if (score < bestPathScore)
                    {
                        bestPathScore  = score;
                        bestPath       = resultPathList;
                        bestFronterier = new Pose(resultPathList.Last().x, resultPathList.Last().y);
                    }
                }
            }


            // convert the best path to command sequence
            if ((bestPath != null) && (bestPath.Count > 2))
            {
                List <Pose> bestPathConv = new List <Pose>();
                bestPathConv.Add(platform.Pose);

                for (int i = 1; i < bestPath.Count; i++)
                {
                    Pose prevPose = bestPathConv.Last();
                    Pose goalPose = new Pose(bestPath[i].x, bestPath[i].y);

                    int dxl = Math.Sign(goalPose.X - prevPose.X);
                    int dyl = Math.Sign(goalPose.Y - prevPose.Y);

                    while (!prevPose.Equals(goalPose)) // it's a bit dangerous here
                    {
                        Pose newPose = new Pose(prevPose.X + dxl, prevPose.Y + dyl);
                        prevPose = newPose;
                        bestPathConv.Add(newPose);
                    }
                }

                for (int i = bestPathConv.Count - 2; i > 0; i--)
                {
                    int    dx     = bestPathConv[i + 1].X - bestPathConv[i].X;
                    int    dy     = bestPathConv[i + 1].Y - bestPathConv[i].Y;
                    double dalpha = Math.Atan2(dy, dx);

                    Pose newPose = new Pose(bestPathConv[i].X, bestPathConv[i].Y, dalpha);
                    commandSequence.Push(newPose);
                }
            }
        }
コード例 #2
0
        private GraphNode FindTrack(Pose startPose, Platform platform, int searchRadius)
        {
            Queue <GraphNode> candidates = new Queue <GraphNode>();

            distMap = Matrix.Create <double>(platform.Map.Rows, platform.Map.Columns, Double.PositiveInfinity);
            GraphNode startNode = new GraphNode(startPose, null, 0, 0);

            candidates.Enqueue(startNode);

            GraphNode bestFronterier      = null;
            double    bestFronterierScore = Double.PositiveInfinity;

            int fronterierNum = 0;

            distMap[startPose.X, startPose.Y] = 0;
            minDistMap = 0;
            maxDistMap = 0;

            // calculate safe zones around platforms
            List <Pose> safeZone = new List <Pose>();

            foreach (Platform plt in platform.ObservedPlatforms)
            {
                RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
                List <Pose>  poses  = limits.GetPosesWithinLimits();
                foreach (Pose p in poses)
                {
                    safeZone.Add(p);
                }
            }

            //graph search
            while (candidates.Count != 0)
            {
                GraphNode cp = candidates.Dequeue();

                int k = cp.Depth + 1;
                if ((k > searchRadius) && (searchRadius != -1))
                {
                    break;
                }
                if (k > maxDeep)
                {
                    break;
                }

                RegionLimits limits = platform.Map.CalculateLimits(cp.Pose.X, cp.Pose.Y, 1);
                List <Pose>  poses  = limits.GetPosesWithinLimits();

                // 1. Generate map based on the previous actions
                MapObject infoMap = platform.Map;

                infoMap = (MapObject)platform.Map.Clone();
                foreach (Pose p in cp.DiscoverCells)
                {
                    // this is an approximation here
                    infoMap.MapMatrix[p.X, p.Y] = 0;
                }

                foreach (Pose p in poses)
                {
                    // is there any other platform on this bin?
                    if (safeZone.Exists(pt => pt.Equals(p)))
                    {
                        continue;
                    }

                    double score = cp.Score + 1;

                    double dalpha = Math.Abs(p.GetHeadingTo(cp.Pose)) / 45.0;
                    score = score + dalpha;

                    // don't override "(distMap[p.X, p.Y] > score)", speed up calculation, this is why this is an approximation algorithm
                    // for this, a correct score/info weighting procedure is needed
                    if (distMap[p.X, p.Y] != Double.PositiveInfinity)
                    {
                        continue;
                    }


                    // 2. Compute info that is gained by the next step
                    // this is an approximation here
                    RegionLimits nlimits = platform.Map.CalculateLimits(p.X, p.Y, 1);
                    List <Pose>  neighp  = nlimits.GetPosesWithinLimits();
                    //double info = neighp.Sum(x => (1 - Math.Abs(infoMap.MapMatrix[x.X, x.Y] - 0.5)) * 2) / 9; // using for-loop instead, cause it's faster

                    // using for-loop instead, cause it's faster
                    double info = 0;
                    for (int i = 0; i < neighp.Count; i++)
                    {
                        info += (1 - Math.Abs(infoMap.MapMatrix[neighp[i].X, neighp[i].Y] - 0.5)) * 2;
                    }
                    info = info / 9;

                    /*List<Tuple<int, Pose>> neighp = platform.CalculateBinsInFOV(p, 2);
                     * double info = neighp.Sum(x => (0.5 - Math.Abs(infoMap.MapMatrix[x.Item2.X, x.Item2.Y] - 0.5)) * 2) / 9;*/

                    score = score - info;

                    // we found a solution if it is not discovered yet
                    if ((platform.Map.MapMatrix[p.X, p.Y] > platform.FreeThreshold) && (platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold))
                    {
                        fronterierNum++;

                        if (((bestFronterierScore > score) && ((allocationMap[p.X, p.Y] == platform.ID))))
                        {
                            bestFronterier               = new GraphNode(p, cp, k, score);
                            bestFronterierScore          = score;
                            bestFronterier.DiscoverCells = new List <Pose>(cp.DiscoverCells);
                            foreach (Pose lp in neighp)
                            {
                                bestFronterier.DiscoverCells.Add(lp);
                            }
                            candidates.Enqueue(bestFronterier);

                            // if search radius is -1, then give back the first fronterier that we found, neverthless the score
                            if (searchRadius == -1)
                            {
                                return(bestFronterier);
                            }

                            continue;
                        }
                    }

                    // this pose is not occupied and has a higher score than the pervious, so expend it
                    if ((platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold) && (distMap[p.X, p.Y] > score))
                    {
                        GraphNode newNode = new GraphNode(p, cp, k, score);
                        newNode.DiscoverCells = new List <Pose>(cp.DiscoverCells);
                        foreach (Pose lp in neighp)
                        {
                            newNode.DiscoverCells.Add(lp);
                        }
                        candidates.Enqueue(newNode);

                        // maintain distance map
                        distMap[p.X, p.Y] = (double)score;
                        if (minDistMap > score)
                        {
                            minDistMap = (double)score;
                        }
                        if (maxDistMap < score)
                        {
                            maxDistMap = (double)score;
                        }
                    }
                }
            }

            return(bestFronterier);
        }
コード例 #3
0
        private GraphNode FindTrack(Pose startPose, Platform platform, int searchRadius)
        {
            Queue <GraphNode> candidates = new Queue <GraphNode>();

            distMap = Matrix.Create <double>(platform.Map.Rows, platform.Map.Columns, Double.PositiveInfinity);
            candidates.Enqueue(new GraphNode(startPose, null, 0, 0));

            GraphNode bestFronterier = null;
            int       fronterierNum  = 0;

            distMap[startPose.X, startPose.Y] = 0;
            minDistMap = 0;
            maxDistMap = 0;

            // calculate safe zones around platforms
            List <Pose> safeZone = new List <Pose>();

            foreach (Platform plt in platform.ObservedPlatforms)
            {
                RegionLimits limits = platform.Map.CalculateLimits(plt.Pose.X, plt.Pose.Y, 1);
                List <Pose>  poses  = limits.GetPosesWithinLimits();
                foreach (Pose p in poses)
                {
                    safeZone.Add(p);
                }
            }

            //graph search
            while (candidates.Count != 0)
            {
                GraphNode cp = candidates.Dequeue();

                int k = cp.Depth + 1;
                if ((k > searchRadius) && (searchRadius != -1))
                {
                    break;
                }
                if (k > maxDeep)
                {
                    break;
                }

                RegionLimits limits = platform.Map.CalculateLimits(cp.Pose.X, cp.Pose.Y, 1);
                List <Pose>  poses  = limits.GetPosesWithinLimits();

                foreach (Pose p in poses)
                {
                    // is there any other platform on this bin?
                    if (safeZone.Exists(pt => pt.Equals(p)))
                    {
                        continue;
                    }

                    double score = cp.Score + 1;

                    double dalpha = Math.Abs(p.GetHeadingTo(cp.Pose)) / 45.0;
                    score = score + dalpha;

                    // we found a solution if it is not discovered yet
                    if ((platform.Map.MapMatrix[p.X, p.Y] > platform.FreeThreshold) && (platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold))
                    {
                        fronterierNum++;

                        if ((bestFronterier == null) || (bestFronterier.Score > score))
                        {
                            bestFronterier = new GraphNode(p, cp, k, score);
                            candidates.Enqueue(bestFronterier);

                            // if search radius is -1, then give back the first fronterier that we found, neverthless the score
                            if (searchRadius == -1)
                            {
                                return(bestFronterier);
                            }
                        }
                    }

                    // this pose is not occupied and has a higher score than the pervious, so expend it
                    if ((platform.Map.MapMatrix[p.X, p.Y] < platform.OccupiedThreshold) && (distMap[p.X, p.Y] > score))
                    {
                        candidates.Enqueue(new GraphNode(p, cp, k, score));

                        // maintain distance map
                        distMap[p.X, p.Y] = (double)score;
                        if (minDistMap > score)
                        {
                            minDistMap = (double)score;
                        }
                        if (maxDistMap < score)
                        {
                            maxDistMap = (double)score;
                        }
                    }
                }
            }

            return(bestFronterier);
        }