private void DoSimpleReceiving(object sender, DoWorkEventArgs e) { if (!(e.Argument is CancellationToken)) { throw new ArgumentException("e.Argument is CancellationToken"); } var cancelToken = (CancellationToken)e.Argument; //receiving loop as long as the subscrition is active or a chancelation is requested while (_subscription.isActive()) { var msg = _subscription.receiveMessage(); if (msg != null) { //Receiving Pedestrian GPS msg using Player service var Stream = msg.getStreamID(); var datatype = (msg.getDataTypeID()); if (datatype == 16002349372918614042) //Datatype with GNSS Datatype { var gnss = (DioneData.Protobuf.GNSS.GlobalPositionData)ProtobufMessageConverter.MessageToProtobuf <DioneData.Protobuf.GNSS.GlobalPositionData>(msg); // var gnss = new DioneData.Protobuf.GNSS.GlobalPositionData(); var lat_gps = gnss.Wgs84Latitude; var lo_gps = gnss.Wgs84Longitude; var height_gps = gnss.Wgs84Height; var wg = new WGS84(); wg.Latitude = lat_gps; wg.Longitude = lo_gps; wg.Height = height_gps; //Covert WGS-84 to UTM coordinates var utm = UTM.FromWGS84(wg, 33); var reader = new HDMapReader(utm, gpsCoordinate_previous); //For storing previous coordinate HISTORIZATION PointQ History_gps = new PointQ(); History_gps.X = utm.X; History_gps.Y = utm.Y; gpsCoordinate_previous = History_gps; //gpsCoordinate_previous.Y = utm.Y; //Console.WriteLine("time stamp: " + time.ToString() + "\t" + "vehicle velocity: " + vel); // Console.WriteLine("WGS-Stream: " + Stream.ToString() + "\t" + "Pedestrian Latitude: " + lat); // Console.WriteLine("WGS-Stream: " + Stream.ToString() + "\t" + "Pedestrian Longitude: " + lo); // Console.WriteLine("WGS-Stream: " + Stream.ToString() + "\t" + "Pedestrian Altitude: " + al); Console.WriteLine("UTM Coordinates of GPS: " + utm); } } if (cancelToken.IsCancellationRequested) { break; } } }
//Pedestrian GPS Point in Road Surface for current Road Surace Determination public static bool IsPointInPolygon(List <PointQ> polygon, PointQ p) { double minX = polygon[0].X; double maxX = polygon[0].X; double minY = polygon[0].Y; double maxY = polygon[0].Y; for (int i = 1; i < polygon.Count; i++) { // Determine Minimum & Maximum boundary points of Road Surface PointQ q = polygon[i]; minX = Math.Min(q.X, minX); maxX = Math.Max(q.X, maxX); minY = Math.Min(q.Y, minY); maxY = Math.Max(q.Y, maxY); } //Jordan curve theorem if (p.X < minX || p.X > maxX || p.Y < minY || p.Y > maxY) { return(false); } bool inside = false; for (int i = 0, j = polygon.Count - 1; i < polygon.Count; j = i++) { if ((polygon[i].Y > p.Y) != (polygon[j].Y > p.Y) && p.X < (polygon[j].X - polygon[i].X) * (p.Y - polygon[i].Y) / (polygon[j].Y - polygon[i].Y) + polygon[i].X) { inside = !inside; } } return(inside); }
public ProbabilityMatrix(PointQ allEndPoints, bool isValid) { this.allEndPoints = allEndPoints; this.isValid = isValid; }
public HDMapReader(UTM utm, PointQ gpsCoordinate_previous) { //Reading HDMap.bin binary file from MapDatabase _mapDatabase = MapDatabase.LoadFromFile("HDMap.bin"); //Extract Road Surfaces form MapDatabase System.Collections.ObjectModel.ReadOnlyCollection <RoadSurface> rsurf = new System.Collections.ObjectModel.ReadOnlyCollection <RoadSurface>(_mapDatabase.GetRoadSurfaces()); //Initialize Pedestrian GPS coordinates of type PoinQ from Recorded Stream PointQ gpsCoordinates = new PointQ { X = utm.X, Y = utm.Y }; Console.WriteLine(); foreach (RoadSurface rs in rsurf) { //Initialize Polygon List List <PointQ> polygon = new List <PointQ>(); //Initialize a list of type PointQ with all possible end points/coordinates List <PointQ> allEndPoints = new List <PointQ>(); //Extract Accessors from Road Surfaces System.Collections.ObjectModel.ReadOnlyCollection <Accessor> accessors = new System.Collections.ObjectModel.ReadOnlyCollection <Accessor>(rs.GetAccessors()); foreach (Accessor a in accessors) { //Console.WriteLine("Id: " + a.Id.ToString()); // Console.WriteLine("LeftRoadSurface: " + a.LeftRoadSurface.ToString()); // Console.WriteLine("Line: " + a.Line.ToString()); // Console.WriteLine("Point A:" + a.Line.Begin.ToString()); // Console.WriteLine("Point B:" + a.Line.End.ToString()); // Console.WriteLine("RightRoadSurface: " + a.RightRoadSurface); // Console.WriteLine("StartingConnections: " + a.StartingConnections.ToString()); //Creat Road surface points using Accessors as boundary points PointQ strt = new PointQ(); strt.X = a.Line.Begin.X; strt.Y = a.Line.Begin.Y; PointQ finish = new PointQ(); finish.X = a.Line.End.X; finish.Y = a.Line.End.Y; polygon.Add(strt); polygon.Add(finish); //Extract Lanes Types System.Collections.Generic.IList <Lane> lane = new List <Lane>(rs.GetLanes()); foreach (Lane l in lane) { } } //Extract Borders System.Collections.Generic.IList <Border> border = new List <Border>(rs.GetBorders()); System.Collections.Generic.IList <IParametricCurve> outline = new List <IParametricCurve>(rs.GetOutline()); //Extract Connections System.Collections.Generic.IList <TUC.Data.HDMap.Connection> connections = new List <TUC.Data.HDMap.Connection>(rs.GetConnections()); foreach (Connection c in connections) { // Console.WriteLine("Connection 1 Begin Point: " + ((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve).Begin.ToString()); // Console.WriteLine("Connection 1 End Point: " + ((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve).End.ToString()); //Inialize Connection Points PointQ begin = new PointQ(); begin.X = ((QuinticHermiteCurve)c.CenterCurve).Begin.X; begin.Y = ((QuinticHermiteCurve)c.CenterCurve).Begin.Y; PointQ end = new PointQ(); end.X = ((QuinticHermiteCurve)c.CenterCurve).End.X; end.Y = ((QuinticHermiteCurve)c.CenterCurve).End.Y; //List<PointQ> allBeginPoints = new List<PointQ>(); // List<PointQ> allEndPoints = new List<PointQ>(); allEndPoints.Add(begin); allEndPoints.Add(end); // Console.WriteLine("All Points:" + allEndPoints.ToString()); //allEndPoints.Add(Connection.endpoint); // add all connection end points like this //AlgorithProbabailty(gpsCoordinates, allPoints); // allEndPoints.Add((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve).Begin; // allEndPoints.Add((QuinticHermiteCurve)c.CenterCurve).Begin; // allEndPoints.Add((TUC.Data.HDMap.QuinticHermiteCurve)c.CenterCurve). } // bool x = IsPointInPolygon(polygon, new PointQ { X = utm.X, Y = utm.Y }); if (x == true) { Console.WriteLine("Pedestriian is Inside of Surface ID: " + rs.Id.ToString()); Console.ReadLine(); predictTrajectory(gpsCoordinates, allEndPoints, gpsCoordinate_previous); } else { Console.WriteLine("Pedestrian is outside of surface ID: " + rs.Id.ToString()); Console.ReadLine(); } // TrajectoryPredictor tp = new TrajectoryPredictor(); // tp.predictTrajectory(gpsCoordinates, allEndPoints); // tp.gpsCoordinate_previous = gpsCoordinates; } }