// Use this for initialization
void Start()
{
string sub_path = (Application.dataPath);
string path = sub_path + "/XMLFiles/roadSignal2.xml";
XmlSerializer serializer = new XmlSerializer(typeof(OpenDRIVE));
string xml = File.ReadAllText(path);
using (var stream = new MemoryStream(Encoding.UTF8.GetBytes(xml)))
{
openDrive = (OpenDRIVE)serializer.Deserialize(stream);
Debug.Log(openDrive.roads[2].signals.signal.s);
}
}
public void SaveToXML(string path)
{
var data = new OpenDRIVE {
};
data.header = new OpenDRIVEHeader();
data.header.revMajor = 1;
data.header.revMajorSpecified = true;
data.header.revMinor = 4;
data.header.revMinorSpecified = true;
data.road = new OpenDRIVERoad[roads.Count];
for (int i = 0; i < roads.Count; ++i)
{
data.road[i] = new OpenDRIVERoad();
data.road[i].id = roads[i].id.ToString();
data.road[i].name = roads[i].name;
data.road[i].length = roads[i].length;
data.road[i].lengthSpecified = true;
data.road[i].junction = roads[i].junctionId;
data.road[i].link = new OpenDRIVERoadLink();
data.road[i].type = new OpenDRIVERoadType[1];
data.road[i].type[0] = new OpenDRIVERoadType();
data.road[i].type[0].s = 0;
data.road[i].type[0].sSpecified = true;
data.road[i].type[0].type = roadType.unknown;
data.road[i].type[0].typeSpecified = true;
// plan view
int planCount = roads[i].lineGeometries.Count;
data.road[i].planView = new OpenDRIVERoadGeometry[planCount];
for (int planIndex = 0; planIndex < planCount; ++planIndex)
{
data.road[i].planView[planIndex] = new OpenDRIVERoadGeometry();
data.road[i].planView[planIndex].s = roads[i].lineGeometries[planIndex].start;
data.road[i].planView[planIndex].sSpecified = true;
data.road[i].planView[planIndex].x = roads[i].lineGeometries[planIndex].xs;
data.road[i].planView[planIndex].xSpecified = true;
data.road[i].planView[planIndex].y = roads[i].lineGeometries[planIndex].ys;
data.road[i].planView[planIndex].ySpecified = true;
data.road[i].planView[planIndex].hdg = roads[i].lineGeometries[planIndex].hdg;
data.road[i].planView[planIndex].hdgSpecified = true;
data.road[i].planView[planIndex].length = roads[i].lineGeometries[planIndex].length;
data.road[i].planView[planIndex].lengthSpecified = true;
data.road[i].planView[planIndex].Items = new OpenDRIVERoadGeometryLine[1];
data.road[i].planView[planIndex].Items[0] = new OpenDRIVERoadGeometryLine();
}
// elevation
int eleCount = roads[i].elevations.Count;
data.road[i].elevationProfile = new OpenDRIVERoadElevationProfile();
data.road[i].elevationProfile.elevation = new OpenDRIVERoadElevationProfileElevation[eleCount];
for (int eleIndex = 0; eleIndex < eleCount; ++eleIndex)
{
data.road[i].elevationProfile.elevation[eleIndex] = new OpenDRIVERoadElevationProfileElevation();
data.road[i].elevationProfile.elevation[eleIndex].s = roads[i].elevations[eleIndex].start;
data.road[i].elevationProfile.elevation[eleIndex].sSpecified = true;
data.road[i].elevationProfile.elevation[eleIndex].a = roads[i].elevations[eleIndex].a;
data.road[i].elevationProfile.elevation[eleIndex].aSpecified = true;
data.road[i].elevationProfile.elevation[eleIndex].b = roads[i].elevations[eleIndex].b;
data.road[i].elevationProfile.elevation[eleIndex].bSpecified = true;
data.road[i].elevationProfile.elevation[eleIndex].c = roads[i].elevations[eleIndex].c;
data.road[i].elevationProfile.elevation[eleIndex].cSpecified = true;
data.road[i].elevationProfile.elevation[eleIndex].d = roads[i].elevations[eleIndex].d;
data.road[i].elevationProfile.elevation[eleIndex].dSpecified = true;
}
// lane
data.road[i].lanes = new OpenDRIVERoadLanes();
int laneCount = roads[i].laneSections.Count;
data.road[i].lanes.laneSection = new OpenDRIVERoadLanesLaneSection[laneCount];
for (int laneIndex = 0; laneIndex < laneCount; ++laneIndex)
{
data.road[i].lanes.laneSection[laneIndex] = new OpenDRIVERoadLanesLaneSection();
data.road[i].lanes.laneSection[laneIndex].s = roads[i].laneSections[laneIndex].start;
data.road[i].lanes.laneSection[laneIndex].sSpecified = true;
// center
data.road[i].lanes.laneSection[laneIndex].center = new OpenDRIVERoadLanesLaneSectionCenter();
data.road[i].lanes.laneSection[laneIndex].center.lane = new centerLane();
data.road[i].lanes.laneSection[laneIndex].center.lane.id = roads[i].laneSections[laneIndex].center.id;
data.road[i].lanes.laneSection[laneIndex].center.lane.idSpecified = true;
data.road[i].lanes.laneSection[laneIndex].center.lane.type = laneType.none;
data.road[i].lanes.laneSection[laneIndex].center.lane.typeSpecified = true;
data.road[i].lanes.laneSection[laneIndex].center.lane.level = singleSide.@false;
data.road[i].lanes.laneSection[laneIndex].center.lane.levelSpecified = true;
data.road[i].lanes.laneSection[laneIndex].center.lane.link = new centerLaneLink();
data.road[i].lanes.laneSection[laneIndex].center.lane.link.predecessor = new centerLaneLinkPredecessor();
data.road[i].lanes.laneSection[laneIndex].center.lane.link.predecessor.id = -1;
data.road[i].lanes.laneSection[laneIndex].center.lane.link.predecessor.idSpecified = true;
// right
data.road[i].lanes.laneSection[laneIndex].right = new OpenDRIVERoadLanesLaneSectionRight();
int rlaneCount = roads[i].laneSections[laneIndex].rights.Count;
data.road[i].lanes.laneSection[laneIndex].right.lane = new lane[rlaneCount];
for (int rIndex = 0; rIndex < rlaneCount; ++rIndex)
{
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex] = new lane();
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].id = roads[i].laneSections[laneIndex].rights[rIndex].id;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].idSpecified = true;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].type = laneType.driving;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].typeSpecified = true;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].level = singleSide.@false;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].levelSpecified = true;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].link = new laneLink();
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].link.predecessor = new laneLinkPredecessor();
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].link.predecessor.id = -1;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].link.predecessor.idSpecified = true;
// width
int widthCount = roads[i].laneSections[laneIndex].rights[rIndex].widths.Count;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].Items = new laneWidth[widthCount];
for (int wIndex = 0; wIndex < widthCount; ++wIndex)
{
laneWidth lw = new laneWidth();
lw.sOffset = roads[i].laneSections[laneIndex].rights[rIndex].widths[wIndex].start;
lw.sOffsetSpecified = true;
lw.a = roads[i].laneSections[laneIndex].rights[rIndex].widths[wIndex].a;
lw.aSpecified = true;
lw.b = roads[i].laneSections[laneIndex].rights[rIndex].widths[wIndex].b;
lw.bSpecified = true;
lw.c = roads[i].laneSections[laneIndex].rights[rIndex].widths[wIndex].c;
lw.cSpecified = true;
lw.d = roads[i].laneSections[laneIndex].rights[rIndex].widths[wIndex].d;
lw.dSpecified = true;
data.road[i].lanes.laneSection[laneIndex].right.lane[rIndex].Items[wIndex] = lw;
}
}
}
}
var serializer = new XmlSerializer(typeof(OpenDRIVE));
using (var stream = new StreamWriter(path))
serializer.Serialize(stream, data);
}
/// <summary>
/// find if point is elligable to be a junction point based on the area of the triangle
/// formed by threee consecutive points
/// </summary>
/// <param name="node1"> </param>
/// <param name="node2"></param>
/// <param name="node3"></param>
/// <returns></returns>
public void SegmentOsmRoads(Dictionary <way, List <node> > wayNodesDict)
{
IList <double> filteredValues = new List <double>();
OpenDRIVE od = new OpenDRIVE();
int p = wayNodesDict.Count();
od.road = new OpenDRIVERoad[p];
int c = 0;
foreach (KeyValuePair <way, List <node> > road in wayNodesDict)
{
// segmentation works on a set of points, which are 3 or more than 3 consecutive points, if otherwise found
// it will be considered as line.
if (road.Value.Count > 3 && road.Key.tag.Find(highWayTag => highWayTag.k == "highway") != null)
{
node nextNode = GetNext <node>(road.Value, road.Value[0]);
var headingsAndDistances = ApplyLSG(road.Value);
double[] curvatureVector = new double[road.Value.Count];
double[] curvatureVector_filtered = new double[road.Value.Count];
double[] distanceVector = new double[road.Value.Count];
double[] headingVector = new double[road.Value.Count];
double[] emaValues = new double[road.Value.Count];
double numberOfnodes = road.Value.Count;
//od.road[c] = odRoad;
for (int x = 0; x < headingsAndDistances.Count(); x++)
{
//TODO: replace distances with headings, it is reversed.
curvatureVector[x] = (headingsAndDistances[x].distance);
Console.WriteLine("Radius of curvature for point {0} is {1}", x, curvatureVector[x]);
}
//curvature/ distance curve will suffer from some data inhereted inaccuricies
//we need to apply smoothing filter to enhance the curve accuracy. Exponential
//moving avarage filter will be used. [SMA : Simple moving avarage for first point only]
//[EMA: Exponential moving avarage], filter periods will be 2.
//EMA Multiplier
double k = 2 / curvatureVector.Count();
for (int i = 0; i < curvatureVector.Count(); i++)
{
distanceVector[i] = headingsAndDistances[i].heading;
}
//filter data, exponential moving average filter
var paramterHandler = new getParamter();
paramterHandler.setAlpha(0.5);
double alpha = paramterHandler.getAlpha();
int curvatureIndx = 0;
foreach (double curvatureVal in curvatureVector)
{
if (curvatureIndx == 0)
{
curvatureVector_filtered[0] = curvatureVal;
}
else
{
curvatureVector_filtered[curvatureIndx] = alpha * curvatureVector[curvatureIndx] + (1 - alpha) * curvatureVector_filtered[curvatureIndx - 1];
}
curvatureIndx++;
}
SegmentationHelper segmentationHandler = new SegmentationHelper(distanceVector, curvatureVector_filtered);
Tuple <List <Osm2Od.Point>, List <int> > junctionPoints = SegmentationHelper.DouglasPeuckerReduction(segmentationHandler.curvaturDistanceDomain, Math.Pow(.007, 1));
double[] junction_Points_x = new double[junctionPoints.Item2.Count()];
double[] junction_Points_y = new double[junctionPoints.Item2.Count()];
for (int i = 0; i < junctionPoints.Item1.Count(); i++)
{
junction_Points_y[i] = junctionPoints.Item1[i].Y;
junction_Points_x[i] = junctionPoints.Item1[i].X;
}
Console.WriteLine("Road {0} junction points calculated, {1} found", road.Key.id, junctionPoints.Item1.Count());
//segment the sections according to line deviataion, if line stills withing our given tolerance, with curvature value
//==0 or near zero, it will be considered a line, if curvature!=0, it is a curve, if != constant value it is a spiral
List <double> slopes = getRoadPrimativeGeometries(junction_Points_x, junction_Points_y);
OpenDRIVERoad odRoad = convertRoadsFromOsmToOd(road, distanceVector, curvatureVector_filtered, junction_Points_x, junction_Points_y);
odRoad.planView = new OpenDRIVERoadGeometry[slopes.Count()];
//create the geometry attribute of the road
for (int i = 0; i < slopes.Count(); i++)
{
OpenDRIVERoadGeometry roadGeometry = new OpenDRIVERoadGeometry();
roadGeometry.Items = new object[1];
roadGeometry.s = distanceVector[junctionPoints.Item2[i]];
roadGeometry.x = MercatorProjection.lonToX(road.Value[junctionPoints.Item2[i]].lon) - MercatorProjection.lonToX(minLon);
roadGeometry.y = MercatorProjection.latToY(road.Value[junctionPoints.Item2[i]].lat) - MercatorProjection.latToY(minLat);
roadGeometry.hdg = headingVector[junctionPoints.Item2[i]];
roadGeometry.length = distanceVector[junctionPoints.Item2[i + 1]];
//TODO: SET CURVATURE TOLERANCE HER
paramterHandler.setCurvatureTol(.00018);
double curvatureTol = paramterHandler.getcurvatureTol();
if (Math.Abs(slopes[i]) <= curvatureTol)
{
if (Math.Abs((curvatureVector_filtered[junctionPoints.Item2[i + 1]] + curvatureVector_filtered[junctionPoints.Item2[i]]) / 2) <= .005)
{
OpenDRIVERoadGeometryLine line = new OpenDRIVERoadGeometryLine();
roadGeometry.Items[0] = line;
}
else
{
OpenDRIVERoadGeometryArc arc = new OpenDRIVERoadGeometryArc();
arc.curvature = curvatureVector_filtered[junctionPoints.Item2[i]];
roadGeometry.Items[0] = arc;
}
}
if (Math.Abs(slopes[i]) > curvatureTol)
{
OpenDRIVERoadGeometrySpiral spiral = new OpenDRIVERoadGeometrySpiral();
spiral.curvStart = curvatureVector_filtered[junctionPoints.Item2[i]];
spiral.curvEnd = curvatureVector_filtered[junctionPoints.Item2[i + 1]];
roadGeometry.Items[0] = spiral;
}
odRoad.planView[i] = roadGeometry;
}
od.road[c] = odRoad;
CurvatureDistanceChart curvatureDistanceChart = new CurvatureDistanceChart(curvatureVector, curvatureVector_filtered, distanceVector, junction_Points_x, junction_Points_y);
}
else
{
Console.WriteLine("[Segmentation] object {0} is a simple line or has no highway tag", road.Key.id);
}
Console.WriteLine("Heading/distance calculated, transforming to curvature/distance domain");
c++;
}
//deserilize to OpenDrive.xodr.
XmlSerializer odDeserilizer = new XmlSerializer(typeof(OpenDRIVE));
var xml = "";
using (var sww = new System.IO.StringWriter())
{
using (System.Xml.XmlWriter writer = System.Xml.XmlWriter.Create(sww))
{
odDeserilizer.Serialize(writer, od);
xml = sww.ToString();
}
}
}
