예제 #1
0
        /// <summary>
        /// Main Constructor for PRMAlgorthim. Creates PRM
        /// </summary>
        /// <param name="map"></param> Playfield Map of the region
        /// <param name="mp"></param>User defined central point in the free space of map
        /// <param name="radius"></param> Radius of the segway
        /// <param name="pointNum"></param> Number of points to be generated in the PRM
        /// <param name="or"></param> User defined estimated ratio of obstacleArea:OveralArea
        public PRMAlgorithm(Field map, double radius, int pointNum)
        {
            PRMmap = map;
            fieldName = map.name;
            mapPoint = PRMmap.freePoint;

            numPRMpoints = pointNum;
            obstacleRatio = PRMmap.AreaRatio();
            //obstacles = PRMmap.GetWallPolygons();
            obstacles = PRMmap.blocks();
            obstacles = BloatObstacles(obstacles, radius);
            obstacles.Add(PRMmap.shape);
            maxDistance = calculateMaxDis();
            createPRM();
            obstacles.Remove(PRMmap.shape);
        }
예제 #2
0
        void IXmlSerializable.ReadXml(XmlReader reader)
        {
            XmlSerializer graphSerializer = new XmlSerializer(typeof(Graph));
            XmlSerializer doubleSerializer = new XmlSerializer(typeof(double));
            XmlSerializer mapSerializer = new XmlSerializer(typeof(Field));
            XmlSerializer vector2Serializer = new XmlSerializer(typeof(Vector2));
            XmlSerializer polygonSerializer = new XmlSerializer(typeof(Polygon));
            XmlSerializer intSerializer = new XmlSerializer(typeof(int));
            bool wasEmpty = reader.IsEmptyElement;
            reader.Read(); // read root tag
            if (wasEmpty)
            {
                return;
            }

            while (reader.NodeType != XmlNodeType.EndElement)
            {
                if (reader.IsStartElement("PRMGraph"))
                {
                    reader.ReadStartElement("PRMGraph");
                    PRMGraph = (Graph)graphSerializer.Deserialize(reader);
                    reader.ReadEndElement();
                }

                else if (reader.IsStartElement("numPRMpoints"))
                {
                    reader.ReadStartElement("numPRMpoints");
                    numPRMpoints = (int)intSerializer.Deserialize(reader);
                    reader.ReadEndElement();
                }

                else if (reader.IsStartElement("PRMmap"))
                {
                    reader.ReadStartElement("PRMmap");
                    PRMmap = (Field)mapSerializer.Deserialize(reader);
                    reader.ReadEndElement();
                }

                else if (reader.IsStartElement("mapPoint"))
                {
                    reader.ReadStartElement("mapPoint");
                    mapPoint = (Vector2)vector2Serializer.Deserialize(reader);
                    reader.ReadEndElement();
                }

                else if (reader.IsStartElement("obstacles"))
                {
                    if (reader.IsEmptyElement)
                    {
                        reader.ReadStartElement("obstacles");
                    }
                    else
                    {
                        reader.ReadStartElement("obstacles");
                        while (reader.NodeType != System.Xml.XmlNodeType.EndElement)
                        {

                            reader.ReadStartElement("obstacle");
                            Polygon p = (Polygon)polygonSerializer.Deserialize(reader);
                            reader.ReadEndElement();
                            obstacles.Add(p);

                        }
                        reader.ReadEndElement();
                    }
                }
                else if (reader.IsStartElement("maxDistance"))
                {
                    reader.ReadStartElement("maxDistance");
                    maxDistance = (double)doubleSerializer.Deserialize(reader);
                    reader.ReadEndElement();
                }
                else if (reader.IsStartElement("obstacleRatio"))
                {
                    reader.ReadStartElement("obstacleRatio");
                    obstacleRatio = (double)doubleSerializer.Deserialize(reader);
                    reader.ReadEndElement();
                }
            }
            reader.ReadEndElement();
        }
예제 #3
0
        /// <summary>
        /// Constructor for the path map generator GUI. Takes the subfield list which is
        /// created by the split function in the main form.
        /// </summary>
        /// <param name="fields">List of subfields</param>
        public PathMapGenerator(List<Field> fields)
        {
            InitializeComponent();

            rend = new Renderer(FieldView);
            wallsRend = new PolygonRenderable(Color.Brown, true);
            objectsRend = new PolygonRenderable(Color.Blue, true);
            regionsRend = new PolygonRenderable(Color.Yellow, 0.5f, true);
            fieldsRend = new PolygonRenderable(Color.Gray);
            pointRend = new PointRenderable();
            pointRend.Show = false;
            lines = new List<PathRenderable>();
            rend.AddRenderable(wallsRend);
            rend.AddRenderable(objectsRend);
            rend.AddRenderable(regionsRend);
            rend.AddRenderable(fieldsRend);
            rend.AddRenderable(pointRend);

            originalTool = rend.Tool;
            polyTool = new ClickTool();
            polyTool.clicked += new EventHandler<ClickedEventArgs>(FieldViewClick);
            rend.Tool = polyTool;
            complete = false;
            PRMs = new List<PRMAlgorithm>();

            rend.OnFormShown();

            this.fields = fields;
            f = fields[0];
            count = 1;
            LoadNext();
        }
예제 #4
0
        /// <summary>
        /// Click to confirm position of the free point. If more fields, call to
        /// load next field. If no more, render the path map. Then close. 
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void OK_Click(object sender, EventArgs e)
        {
            if (complete)
            {
                this.Hide();
                return;
            }
            f.DefineFreePoint(point);
            PRMs.Add(new PRMAlgorithm(f, 0.35, (int)(f.AreaDifference() / 0.2)));
            if (++count <= fields.Count)
            {
                f = fields[count - 1];
                LoadNext();
            }
            else              //if all points found render all lines
            {
                reset();
                Number_Label.Text = "Path Map and PRM";
                OK.Text = "Save";
                Vector2 center = new Vector2(0, 0);
                List<Polygon> fieldsComp = new List<Polygon>();
                List<Polygon> wallsComp = new List<Polygon>();
                List<Polygon> objectsComp = new List<Polygon>();
                List<Polygon> regionsComp = new List<Polygon>();
                foreach (Field ff in fields)
                {
                    fieldsComp.Add(ff.shape);
                    TextRenderable name = new TextRenderable(ff.name, new PointF((float)ff.shape.Center.X, (float)ff.shape.Center.Y), Color.Black);
                    rend.AddRenderable(name);
                    center += ff.shape.Center;
                    foreach (List<Polygon> p in ff.walls.Values)
                    {
                        wallsComp.AddRange(p);
                    }
                    foreach (List<Polygon> p in ff.objects.Values)
                    {
                        objectsComp.AddRange(p);
                    }
                    foreach (List<Polygon> p in ff.regions.Values)
                    {
                        regionsComp.AddRange(p);
                    }
                }
                fieldsRend.Polygons = fieldsComp;
                wallsRend.Polygons = wallsComp;
                objectsRend.Polygons = objectsComp;
                regionsRend.Polygons = regionsComp;
                center /= fields.Count;
                rend.CamOrtho.CameraPosition = new PoseYPR(center.X, center.Y, rend.CamOrtho.CameraPosition.z, rend.CamOrtho.CameraPosition.yaw,
                    rend.CamOrtho.CameraPosition.pitch, rend.CamOrtho.CameraPosition.roll);
                OK.Enabled = true;
                complete = true;

                pm = new PRMNavigationPlanner(PRMs, "").getPathMap();

                foreach (PRMAlgorithm prm in PRMs)
                {
                    foreach (Vector2 v in prm.getNodeVector2())
                    {
                        PointRenderable pointr = new PointRenderable(v);
                        rend.AddRenderable(pointr);
                    }
                }
                foreach (PathMapNode pmn in pm.map)
                {
                    PathRenderable line = new PathRenderable(new List<Vector2>() { pmn.v1.Item1, pmn.v2.Item1 });
                    line.DisplayColor = Color.Blue;
                    lines.Add(line);
                    rend.AddRenderable(line);
                }
            }
        }
예제 #5
0
 /// <summary>
 /// This occurs when the renderer's tool is set to clickTool. It gets the
 /// location of the mouseclick relative to the rederer.
 /// </summary>
 /// <param name="sender"></param>
 /// <param name="e"></param>
 private void FieldViewClick(object sender, ClickedEventArgs e)
 {
     if (!complete)
     {
         bool outside = !f.shape.IsInside(e.Location);
         if (outside)
         {
             reset();
             Error_OutsideField.Show();
         }
         bool inside = false;
         foreach (Polygon p in f.blocks())
         {
             if (p.IsInside(e.Location))
             {
                 inside = true;
                 break;
             }
         }
         if (!outside && inside)
         {
             reset();
             Error_InsideItem.Show();
         }
         if (!outside && !inside)
         {
             reset();
             point = e.Location;
             pointRend.Coordinates2D = e.Location;
             pointRend.Show = true;
             OK.Enabled = true;
         }
     }
     else
     {
         bool outside = true;
         bool inside = false;
         foreach (Field s in fields)
         {
             if (s.shape.IsInside(e.Location))
             {
                 outside = false;
                 f = s;
             }
             foreach (Polygon p in s.blocks())
             {
                 if (p.IsInside(e.Location))
                 {
                     inside = true;
                     break;
                 }
             }
         }
         if (outside)
         {
             reset();
             OK.Enabled = true;
             Error_OutsideField.Show();
         }
         else if (inside)
         {
             reset();
             OK.Enabled = true;
             Error_InsideItem.Show();
         }
         else
         {
             PRMs[fields.IndexOf(f)].addPoint(e.Location.X, e.Location.Y);
             PointRenderable p = new PointRenderable(e.Location);
             rend.AddRenderable(p);
             pm = new PRMNavigationPlanner(PRMs, "").getPathMap();
             foreach (PathRenderable pr in lines)
             {
                 rend.RemoveRenderable(pr);
             }
             foreach (PathMapNode pmn in pm.map)
             {
                 PathRenderable line = new PathRenderable(new List<Vector2>() { pmn.v1.Item1, pmn.v2.Item1 });
                 line.DisplayColor = Color.Blue;
                 lines.Add(line);
                 rend.AddRenderable(line);
             }
         }
     }
 }
예제 #6
0
 /// <summary>
 /// Grabs all the objects, regions, and walls inside the field polygon 
 /// and stores them in a new Field.
 /// </summary>
 /// <param name="field">The name and shape of the area desired for the 
 /// new field.</param>
 /// <returns>A new Field that is a subsection of mainField</returns>
 private Field split(KeyValuePair<string, Polygon> field)
 {
     Field subField = new Field(field.Key);
     subField.shape = field.Value;
     foreach (KeyValuePair<string, Polygon> f in subFields)
     {
         foreach (Vector2 v in f.Value)
         {
             if (checkInside(v))
             {
                 Error inside = new Error(errorTypes.SaveInside, f.Key);
                 inside.ShowDialog();
                 inside.Dispose();
                 return null;
             }
         }
     }
     foreach (KeyValuePair<string, List<Polygon>> w in mainField.walls)
     {
         List<Polygon> subWalls = new List<Polygon>();
         foreach (Polygon p in w.Value)
         {
             List<Polygon> sub = splitHelp(field.Value, p);
             if (sub.Count != 0)
             {
                 subWalls.AddRange(sub);
             }
         }
         if (subWalls.Count != 0)
         {
             if (w.Key.Contains(" : "))
             {
                 int pos = w.Key.IndexOf(" : ");
                 subField.addWall(w.Key.Substring(0, pos) + " : " + field.Key, subWalls);
             }
             else
             {
                 subField.addWall(w.Key + " : " + field.Key, subWalls);
             }
         }
     }
     foreach (KeyValuePair<string, List<Polygon>> o in mainField.objects)
     {
         List<Polygon> subObjects = new List<Polygon>();
         foreach (Polygon p in o.Value)
         {
             List<Polygon> sub = splitHelp(field.Value, p);
             if (sub.Count != 0)
             {
                 subObjects.AddRange(sub);
             }
         }
         if (subObjects.Count != 0)
         {
             if (o.Key.Contains(" : "))
             {
                 int pos = o.Key.IndexOf(" : ");
                 subField.addObject(o.Key.Substring(0, pos) + " : " + field.Key, subObjects, 0);
             }
             else
             {
                 subField.addObject(o.Key + " : " + field.Key, subObjects, 0);
             }
         }
     }
     foreach (KeyValuePair<string, List<Polygon>> r in mainField.regions)
     {
         List<Polygon> subRegions = new List<Polygon>();
         foreach (Polygon p in r.Value)
         {
             List<Polygon> sub = splitHelp(field.Value, p);
             if (sub.Count != 0)
             {
                 subRegions.AddRange(sub);
             }
         }
         if (subRegions.Count != 0)
         {
             if (r.Key.Contains(" : "))
             {
                 int pos = r.Key.IndexOf(" : ");
                 subField.addRegion(r.Key.Substring(0, pos) + " : " + field.Key, subRegions);
             }
             else
             {
                 subField.addRegion(r.Key + " : " + field.Key, subRegions);
             }
         }
     }
     return subField;
 }
예제 #7
0
        public MainGUI()
        {
            InitializeComponent();
            rend = new Renderer(FieldView);
            ocgridRend = new OccupancyGridRenderable("Grid SLAM Map", Color.Red);
            firstPointRend = new PointRenderable();
            firstPointRend.Show = false;
            newPolyRend = new PathRenderable();
            wallsRend = new PolygonRenderable(Color.Brown, true);
            objectsRend = new PolygonRenderable(Color.Blue, true);
            regionsRend = new PolygonRenderable(Color.Yellow, 0.5f, true);
            fieldsRend = new PolygonRenderable(Color.Gray);
            selectedPolyRend = new PolygonRenderable(Color.Black);
            selectedPolyRend.LineThickness = 2F;
            rend.AddRenderable(ocgridRend);
            rend.AddRenderable(firstPointRend);
            rend.AddRenderable(newPolyRend);
            rend.AddRenderable(wallsRend);
            rend.AddRenderable(objectsRend);
            rend.AddRenderable(regionsRend);
            rend.AddRenderable(fieldsRend);
            rend.AddRenderable(selectedPolyRend);

            mainField = new Field("main");
            subFields = new Dictionary<string, Polygon>();
            selectedPoly = new List<Polygon>();

            originalTool = rend.Tool;
            polyTool = new ClickTool();
            polyTool.clicked += new EventHandler<ClickedEventArgs>(FieldViewClick);

            newPoly = new Polygon();
            accRobot = false;
            ocgridShow = true;

            Ctrl_press.Hide();
        }