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