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