示例#1
0
        //function that calculates all the intermediate points between each turning point (or jumpPoint if you may)
        private void DrawPoints(GridLine x, int agvIndex)
        {
            //think of the incoming GridLine as follows:
            //If you want to move from A to B, there might be an obstacle in the way, which must be bypassed
            //For this purpose, there must be found a point to break the final route into 2 smaller (let's say A->b + b->B (AB in total)
            //The incoming GridLine contains the pair of Coordinates for each one of the smaller routes
            //So, for our example, GridLine x containts the starting A(x,y) & b(x,y)
            //In a nutshell, this functions calculates all the child-steps of the parent-Line, determined by x.fromX,x.fromY and x.toX,x.toY


            //the parent-Line will finaly consist of many pairs of (x,y): e.g [X1,Y1 / X2,Y2 / X3,Y3 ... Xn,Yn]

            var    x1       = x.FromX;
            var    y1       = x.FromY;
            var    x2       = x.ToX;
            var    y2       = x.ToY;
            double distance = _f.GetLength(x1, y1, x2, y2); //function that returns the Euclidean distance between 2 points

            double side = _f.getSide(_rectangles[0][0].Height
                                     , _rectangles[0][0].Height); //function that returns the hypotenuse of a GridBox

            int distanceBlocks = -1;                              //the quantity of blocks,matching the current line's length

            //Calculates the number of GridBoxes that the Line consists of. Calculation depends on 2 scenarios:
            //Scenario 1: Line is Diagonal
            //Scenario 2: Line is Straight
            if ((x1 < x2) && (y1 < y2)) //diagonal-right bottom direction
            {
                distanceBlocks = Convert.ToInt32(distance / side);
            }
            else if ((x1 < x2) && (y1 > y2)) //diagonal-right top direction
            {
                distanceBlocks = Convert.ToInt32(distance / side);
            }
            else if ((x1 > x2) && (y1 < y2)) //diagonal-left bottom direction
            {
                distanceBlocks = Convert.ToInt32(distance / side);
            }
            else if ((x1 > x2) && (y1 > y2)) //diagonal-left top direction
            {
                distanceBlocks = Convert.ToInt32(distance / side);
            }
            else if ((y1 == y2) || (x1 == x2)) //horizontal or vertical
            {
                distanceBlocks = Convert.ToInt32(distance / _rectangles[0][0].Width);
            }
            else
            {
                MessageBox.Show(this, "Unexpected error", "", MessageBoxButtons.OK, MessageBoxIcon.Error);
            }

            //1d array of points.used to track all the points of current line
            Point[] currentLinePoints = new Point[distanceBlocks];

            //here we calculate the X,Y coordinates of all the intermediate points
            for (var i = 0; i < distanceBlocks; i++)
            {
                _calibrated = false;
                double t;
                if (distance != 0) //obviously, distance cannot be zero
                {
                    t = ((side) / distance);
                }
                else
                {
                    return;
                }

                //these are the x,y coord that are calculated in every for-loop
                _a = Convert.ToInt32(((1 - t) * x1) + (t * x2));
                _b = Convert.ToInt32(((1 - t) * y1) + (t * y2));
                Point p = new Point(_a, _b); //merges the calculated x,y into 1 Point variable

                for (var k = 0; k < Globals.WidthBlocks; k++)
                {
                    for (var l = 0; l < Globals.HeightBlocks; l++)
                    {
                        if (_rectangles[k][l].BoxRec.Contains(p))   //this is how we assign the previously calculated pair of X,Y to a GridBox

                        //a smart way to handle GridBoxes from their center
                        {
                            int sideX = _rectangles[k][l].BoxRec.X + ((Globals.BlockSide / 2) - 1);
                            int sideY = _rectangles[k][l].BoxRec.Y + ((Globals.BlockSide / 2) - 1);
                            currentLinePoints[i].X = sideX;
                            currentLinePoints[i].Y = sideY;

                            if (dotsToolStripMenuItem.Checked)
                            {
                                using (SolidBrush br = new SolidBrush(Color.BlueViolet))
                                    _paper.FillEllipse(br, currentLinePoints[i].X - 3,
                                                       currentLinePoints[i].Y - 3,
                                                       5, 5);
                            }

                            using (Font stepFont = new Font("Tahoma", 8, FontStyle.Bold))//Font used for numbering the steps/current block)
                            {
                                using (SolidBrush fontBr = new SolidBrush(Color.FromArgb(53, 153, 153)))
                                    if (stepsToolStripMenuItem.Checked)
                                    {
                                        _paper.DrawString(_AGVs[agvIndex].StepsCounter + ""
                                                          , stepFont
                                                          , fontBr
                                                          , currentLinePoints[i]);
                                    }
                            }
                            _calibrated = true;
                        }
                    }
                }

                if (_calibrated)   //for each one of the above calculations, we check if the calibration has been done correctly and, if so, each pair is inserted to the corresponding AGV's steps List
                {
                    _AGVs[agvIndex].Steps[_AGVs[agvIndex].StepsCounter].X = currentLinePoints[i].X;
                    _AGVs[agvIndex].Steps[_AGVs[agvIndex].StepsCounter].Y = currentLinePoints[i].Y;
                    _AGVs[agvIndex].StepsCounter++;
                }
                //initialize next steps.
                x1       = currentLinePoints[i].X;
                y1       = currentLinePoints[i].Y;
                distance = _f.GetLength(x1, y1, x2, y2);
            }
        }
示例#2
0
        //Basic path planner function
        private void Redraw()
        {
            bool startFound = false;
            bool endFound   = false;

            _mapHasLoads = false;

            GridPos endPos = new GridPos();

            _posIndex = 0;
            _startPos = new List <GridPos>(); //list that will be filled with the starting points of every AGV
            _AGVs     = new List <Vehicle>(); //list that will be filled with objects of the class Vehicle
            _loadPos  = new List <GridPos>(); //list that will be filled with the points of every Load
            _loads    = 0;
            //Double FOR-loops to scan the whole Grid and perform the needed actions
            for (var i = 0; i < Globals.WidthBlocks; i++)
            {
                for (var j = 0; j < Globals.HeightBlocks; j++)
                {
                    if (_rectangles[i][j].BoxType == BoxType.Wall)
                    {
                        _searchGrid.SetWalkableAt(new GridPos(i, j), false);//Walls are marked as non-walkable
                    }
                    else
                    {
                        _searchGrid.SetWalkableAt(new GridPos(i, j), true);//every other block is marked as walkable (for now)
                    }
                    if (_rectangles[i][j].BoxType == BoxType.Load)
                    {
                        _mapHasLoads = true;
                        _searchGrid.SetWalkableAt(new GridPos(i, j), false); //marks every Load as non-walkable
                        _isLoad[i, j] = 1;                                   //considers every Load as available
                        _loads++;                                            //counts the number of available Loads in the grid
                        _loadPos.Add(new GridPos(i, j));                     //inserts the coordinates of the Load inside a list
                    }
                    if (_rectangles[i][j].BoxType == BoxType.Normal)
                    {
                        _rectangles[i][j].OnHover(_boxDefaultColor);
                    }

                    if (_rectangles[i][j].BoxType == BoxType.Start)
                    {
                        if (_beforeStart)
                        {
                            _searchGrid.SetWalkableAt(new GridPos(i, j), false); //initial starting points of AGV are non walkable until 1st run is completed
                        }
                        else
                        {
                            _searchGrid.SetWalkableAt(new GridPos(i, j), true);
                        }

                        startFound = true;

                        _AGVs.Add(new Vehicle(this));
                        _AGVs[_posIndex].ID = _posIndex;

                        _startPos.Add(new GridPos(i, j)); //adds the starting coordinates of an AGV to the StartPos list

                        //a & b are used by DrawPoints() as the starting x,y for calculation purposes
                        _a = _startPos[_posIndex].X;
                        _b = _startPos[_posIndex].Y;

                        if (_posIndex < _startPos.Count)
                        {
                            _startPos[_posIndex] = new GridPos(_startPos[_posIndex].X, _startPos[_posIndex].Y);
                            _posIndex++;
                        }
                    }

                    if (_rectangles[i][j].BoxType == BoxType.End)
                    {
                        endFound        = true;
                        endPos.X        = i;
                        endPos.Y        = j;
                        _endPointCoords = new Point(i * Globals.BlockSide, j * Globals.BlockSide + Globals.TopBarOffset);
                    }
                }
            }



            if (!startFound || !endFound)
            {
                return; //will return if there are no starting or end points in the Grid
            }
            _posIndex = 0;

            if (_AGVs != null)
            {
                for (short i = 0; i < _AGVs.Count(); i++)
                {
                    if (_AGVs[i] != null)
                    {
                        _AGVs[i].UpdateAGV();
                        _AGVs[i].Status.Busy = false; //initialize the status of _AGVs, as 'available'
                    }
                }
            }

            _startPos = NotTrappedVehicles(_startPos, endPos); //replaces the List with all the inserted _AGVs
                                                               //with a new one containing the right ones
            if (_mapHasLoads)
            {
                KeepValidLoads(endPos); //calls a function that checks which Loads are available
            }
            //to be picked up by _AGVs and removed the trapped ones.


            //For-loop to repeat the path-finding process for ALL the _AGVs that participate in the simulation
            for (short i = 0; i < _startPos.Count; i++)
            {
                if (_loadPos.Count != 0)
                {
                    var task = System.Threading.Tasks.Task.Run(() => CheckForTrappedLoads(_loadPos, endPos));
                    _loadPos = task.Result;
                    //_loadPos = await CheckForTrappedLoads(_loadPos, endPos);
                }
                if (_loadPos.Count == 0)
                {
                    _mapHasLoads           = false;
                    _AGVs[i].HasLoadToPick = false;
                }
                else
                {
                    _mapHasLoads           = true;
                    _AGVs[i].HasLoadToPick = true;
                }


                if (_AGVs[i].Status.Busy == false)
                {
                    List <GridPos> jumpPointsList;
                    switch (_mapHasLoads)
                    {
                    case true:
                        //====create the path FROM START TO LOAD, if load exists=====
                        for (int m = 0; m < _loadPos.Count; m++)
                        {
                            _searchGrid.SetWalkableAt(_loadPos[m], false);     //Do not allow walk over any other load except the targeted one
                        }
                        _searchGrid.SetWalkableAt(_loadPos[0], true);

                        //use of the A* alorithms to find the path between AGV and its marked Load
                        _jumpParam.Reset(_startPos[_posIndex], _loadPos[0]);
                        jumpPointsList       = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
                        _AGVs[i].JumpPoints  = jumpPointsList;
                        _AGVs[i].Status.Busy = true;
                        //====create the path FROM START TO LOAD, if load exists=====

                        //======FROM LOAD TO END======
                        for (int m = 0; m < _loadPos.Count; m++)
                        {
                            _searchGrid.SetWalkableAt(_loadPos[m], false);
                        }
                        _jumpParam.Reset(_loadPos[0], endPos);
                        jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
                        _AGVs[i].JumpPoints.AddRange(jumpPointsList);

                        //marks the load that each AGV picks up on the 1st route, as 3, so each agv knows where to go after delivering the 1st load
                        _isLoad[_loadPos[0].X, _loadPos[0].Y] = 3;
                        _AGVs[i].MarkedLoad = new Point(_loadPos[0].X, _loadPos[0].Y);

                        _loadPos.Remove(_loadPos[0]);
                        //======FROM LOAD TO END======
                        break;

                    case false:
                        _jumpParam.Reset(_startPos[_posIndex], endPos);
                        jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);

                        _AGVs[i].JumpPoints = jumpPointsList;
                        break;
                    }
                }
                _posIndex++;
            }

            int c = 0;

            for (short i = 0; i < _startPos.Count; i++)
            {
                c += _AGVs[i].JumpPoints.Count;
            }


            for (short i = 0; i < _startPos.Count; i++)
            {
                for (int j = 0; j < _AGVs[i].JumpPoints.Count - 1; j++)
                {
                    GridLine line = new GridLine
                                    (
                        _rectangles[_AGVs[i].JumpPoints[j].X][_AGVs[i].JumpPoints[j].Y],
                        _rectangles[_AGVs[i].JumpPoints[j + 1].X][_AGVs[i].JumpPoints[j + 1].Y]
                                    );

                    _AGVs[i].Paths[j] = line;
                }
            }

            for (int i = 0; i < _startPos.Count; i++)
            {
                if ((c - 1) > 0)
                {
                    Array.Resize(ref _AGVs[i].Paths, c - 1); //resize of the _AGVs steps Table
                }
            }
            if (_loads != 0)
            {
                tree_stats.Nodes[2].Text = "Remaining loads: " + _loads;
            }
            else
            {
                tree_stats.Nodes[2].Text = "Remaining loads: ";
            }
            Invalidate();
        }
示例#3
0
        //Path-planner for collecting all the remaining Loads in the Grid
        private void GetNextLoad(int whichAgv)
        {
            aGVIndexToolStripMenuItem.Checked = false;
            GridPos endPos = new GridPos();


            //finds the End point and uses it's coordinates as the starting coords for every AGV
            for (var widthTrav = 0; widthTrav < Globals.WidthBlocks; widthTrav++)
            {
                for (var heightTrav = 0; heightTrav < Globals.HeightBlocks; heightTrav++)
                {
                    if (_rectangles[widthTrav][heightTrav].BoxType == BoxType.End)
                    {
                        try {
                            _startPos[whichAgv] = new GridPos(widthTrav, heightTrav);
                            _a = _startPos[whichAgv].X;
                            _b = _startPos[whichAgv].Y;
                        } catch { }
                    }
                }
            }

            List <GridPos> loadPos = new List <GridPos>();

            for (var i = 0; i < Globals.WidthBlocks; i++)
            {
                for (var j = 0; j < Globals.HeightBlocks; j++)
                {
                    if (_rectangles[i][j].BoxType == BoxType.Load)
                    {
                        _searchGrid.SetWalkableAt(new GridPos(i, j), false);
                    }

                    //places the available AND the temporarily trapped loads in a list
                    if (_isLoad[i, j] == 1 || _isLoad[i, j] == 4)
                    {
                        loadPos.Add(new GridPos(i, j));
                    }
                }
            }
            loadPos = CheckForTrappedLoads(loadPos, new GridPos(_a, _b), true); //scans the loadPos list to check which loads are available
            if (loadPos.Count == 0)
            {
                _AGVs[whichAgv].HasLoadToPick = false;
                return;
            }
            _isLoad[loadPos[0].X, loadPos[0].Y] = 3;
            _AGVs[whichAgv].MarkedLoad          = new Point(loadPos[0].X, loadPos[0].Y);
            _loads--;
            endPos = loadPos[0];

            //Mark all loads as unwalkable,except the targetted ones
            for (var m = 0; m < loadPos.Count; m++)
            {
                _searchGrid.SetWalkableAt(loadPos[m], false);
            }
            _searchGrid.SetWalkableAt(loadPos[0], true);

            //creates the path between the AGV (which at the moment is at the exit) and the Load
            _jumpParam.Reset(_startPos[whichAgv], endPos);
            List <GridPos> jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);

            _AGVs[whichAgv].JumpPoints = jumpPointsList; //adds the result from A* to the AGV's
                                                         //embedded List

            //Mark all loads as unwalkable
            for (var m = 0; m < loadPos.Count; m++)
            {
                _searchGrid.SetWalkableAt(loadPos[m], false);
            }

            int c = 0;

            for (short i = 0; i < _startPos.Count; i++)
            {
                c += _AGVs[i].JumpPoints.Count;
                if ((c - 1) > 0)
                {
                    Array.Resize(ref _AGVs[i].Paths, c - 1);
                }
            }


            for (int j = 0; j < _AGVs[whichAgv].JumpPoints.Count - 1; j++)
            {
                GridLine line = new GridLine(
                    _rectangles
                    [_AGVs[whichAgv].JumpPoints[j].X]
                    [_AGVs[whichAgv].JumpPoints[j].Y],
                    _rectangles
                    [_AGVs[whichAgv].JumpPoints[j + 1].X]
                    [_AGVs[whichAgv].JumpPoints[j + 1].Y]
                    );

                _AGVs[whichAgv].Paths[j] = line;
            }


            //2nd part of route: Go to exit
            int oldC = c - 1;

            _jumpParam.Reset(endPos, _startPos[whichAgv]);
            jumpPointsList = AStarFinder.FindPath(_jumpParam, Globals.AStarWeight);
            _AGVs[whichAgv].JumpPoints.AddRange(jumpPointsList);

            c = 0;
            for (int i = 0; i < _startPos.Count; i++)
            {
                c += _AGVs[i].JumpPoints.Count;
                if ((c - 1) > 0)
                {
                    Array.Resize(ref _AGVs[i].Paths, oldC + (c - 1));
                }
            }


            for (short i = 0; i < _startPos.Count; i++)
            {
                for (int j = 0; j < _AGVs[i].JumpPoints.Count - 1; j++)
                {
                    GridLine line = new GridLine(
                        _rectangles
                        [_AGVs[i].JumpPoints[j].X]
                        [_AGVs[i].JumpPoints[j].Y],
                        _rectangles
                        [_AGVs[i].JumpPoints[j + 1].X]
                        [_AGVs[i].JumpPoints[j + 1].Y]
                        );

                    _AGVs[i].Paths[j] = line;
                }
            }

            Invalidate();
        }
示例#4
0
        //Basic path planner function
        private void Redraw()
        {
            return;

            bool start_found = false;
            bool end_found   = false;

            mapHasLoads = false;

            GridPos endPos = new GridPos();

            pos_index = 0;
            startPos  = new List <GridPos>(); //list that will be filled with the starting points of every AGV
            AGVs      = new List <Vehicle>(); //list that will be filled with objects of the class Vehicle
            loadPos   = new List <GridPos>(); //list that will be filled with the points of every Load


            //Double FOR-loops to scan the whole Grid and perform the needed actions
            for (int i = 0; i < Globals._WidthBlocks; i++)
            {
                for (int j = 0; j < Globals._HeightBlocks; j++)
                {
                    if (m_rectangles[i][j].boxType == BoxType.Wall)
                    {
                        searchGrid.SetWalkableAt(new GridPos(i, j), false);//Walls are marked as non-walkable
                    }
                    else
                    {
                        searchGrid.SetWalkableAt(new GridPos(i, j), true);//every other block is marked as walkable (for now)
                    }
                    if (m_rectangles[i][j].boxType == BoxType.Load)
                    {
                        mapHasLoads = true;
                        searchGrid.SetWalkableAt(new GridPos(i, j), false); //marks every Load as non-walkable
                        isLoad[i, j] = 1;                                   //considers every Load as available
                        loadPos.Add(new GridPos(i, j));                     //inserts the coordinates of the Load inside a list
                    }
                    if (m_rectangles[i][j].boxType == BoxType.Normal)
                    {
                        m_rectangles[i][j].onHover(Globals.boxDefaultColor);
                    }

                    if (m_rectangles[i][j].boxType == BoxType.Start)
                    {
                        if (beforeStart)
                        {
                            searchGrid.SetWalkableAt(new GridPos(i, j), false); //initial starting points of AGV are non walkable until 1st run is completed
                        }
                        else
                        {
                            searchGrid.SetWalkableAt(new GridPos(i, j), true);
                        }

                        start_found = true;

                        AGVs.Add(new Vehicle(this));
                        AGVs[pos_index].ID = pos_index;

                        startPos.Add(new GridPos(i, j)); //adds the starting coordinates of an AGV to the StartPos list

                        //a & b are used by DrawPoints() as the starting x,y for calculation purposes
                        a = startPos[pos_index].x;
                        b = startPos[pos_index].y;

                        if (pos_index < startPos.Count)
                        {
                            startPos[pos_index] = new GridPos(startPos[pos_index].x, startPos[pos_index].y);
                            pos_index++;
                        }
                    }

                    if (m_rectangles[i][j].boxType == BoxType.End)
                    {
                        end_found = true;
                        endPos.x  = i;
                        endPos.y  = j;
                    }
                }
            }



            if (!start_found || !end_found)
            {
                return; //will return if there are no starting or end points in the Grid
            }
            pos_index = 0;

            if (AGVs != null)
            {
                for (int i = 0; i < AGVs.Count(); i++)
                {
                    if (AGVs[i] != null)
                    {
                        AGVs[i].Status.Busy = false; //initialize the status of AGVs, as 'available'
                    }
                }
            }

            startPos = NotTrappedVehicles(startPos, endPos); //replaces the List with all the inserted AGVs
                                                             //with a new one containing the right ones
            if (mapHasLoads)
            {
                KeepValidLoads(endPos); //calls a function that checks which Loads are available
            }
            //to be picked up by AGVs and removed the trapped ones.


            //For-loop to repeat the path-finding process for ALL the AGVs that participate in the simulation
            for (int i = 0; i < startPos.Count; i++)
            {
                if (loadPos.Count != 0)
                {
                    loadPos = CheckForTrappedLoads(loadPos, endPos);
                }

                if (loadPos.Count == 0)
                {
                    mapHasLoads           = false;
                    AGVs[i].HasLoadToPick = false;
                }
                else
                {
                    mapHasLoads           = true;
                    AGVs[i].HasLoadToPick = true;
                }


                if (AGVs[i].Status.Busy == false)
                {
                    List <GridPos> JumpPointsList;
                    switch (mapHasLoads)
                    {
                    case true:
                        //====create the path FROM START TO LOAD, if load exists=====
                        for (int m = 0; m < loadPos.Count; m++)
                        {
                            searchGrid.SetWalkableAt(loadPos[m], false);     //Do not allow walk over any other load except the targeted one
                        }
                        searchGrid.SetWalkableAt(loadPos[0], true);

                        //use of the A* alorithms to find the path between AGV and its marked Load
                        jumpParam.Reset(startPos[pos_index], loadPos[0]);
                        JumpPointsList      = AStarFinder.FindPath(jumpParam, nud_weight.Value);
                        AGVs[i].JumpPoints  = JumpPointsList;
                        AGVs[i].Status.Busy = true;
                        //====create the path FROM START TO LOAD, if load exists=====

                        //======FROM LOAD TO END======
                        for (int m = 0; m < loadPos.Count; m++)
                        {
                            searchGrid.SetWalkableAt(loadPos[m], false);
                        }
                        jumpParam.Reset(loadPos[0], endPos);
                        JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value);
                        AGVs[i].JumpPoints.AddRange(JumpPointsList);

                        //marks the load that each AGV picks up on the 1st route, as 3, so each agv knows where to go after delivering the 1st load
                        isLoad[loadPos[0].x, loadPos[0].y] = 3;
                        AGVs[i].MarkedLoad = new Point(loadPos[0].x, loadPos[0].y);

                        loadPos.Remove(loadPos[0]);
                        //======FROM LOAD TO END======
                        break;

                    case false:
                        jumpParam.Reset(startPos[pos_index], endPos);
                        JumpPointsList = AStarFinder.FindPath(jumpParam, nud_weight.Value);

                        AGVs[i].JumpPoints = JumpPointsList;
                        break;
                    }
                }
                pos_index++;
            }

            int c = 0;

            for (int i = 0; i < startPos.Count; i++)
            {
                c += AGVs[i].JumpPoints.Count;
            }


            for (int i = 0; i < startPos.Count; i++)
            {
                for (int j = 0; j < AGVs[i].JumpPoints.Count - 1; j++)
                {
                    GridLine line = new GridLine
                                    (
                        m_rectangles[AGVs[i].JumpPoints[j].x][AGVs[i].JumpPoints[j].y],
                        m_rectangles[AGVs[i].JumpPoints[j + 1].x][AGVs[i].JumpPoints[j + 1].y]
                                    );

                    AGVs[i].Paths[j] = line;
                }
            }

            for (int i = 0; i < startPos.Count; i++)
            {
                if ((c - 1) > 0)
                {
                    Array.Resize(ref AGVs[i].Paths, c - 1); //resize of the AGVs steps Table
                }
            }
            Invalidate();
        }