public static Observation vectorToObservation(Vector2D v, bool isOccupied, Vector2D rbotPos, double rbotAngle, int unitLength, int certainty) { v.rotate(rbotAngle - 90); v.translate(rbotPos); v.scale(1.0 / unitLength); return new Observation((int)v.getX(), (int)v.getY(), isOccupied, certainty); }
public List<List<Vector2D>> getPointsToCheck() { if (pointsToCheck != null) return pointsToCheck; pointsToCheck = new List<List<Vector2D>>(); List<Vector2D> listOfCoords = new List<Vector2D>(); String sLine = ""; StreamReader objReader = new StreamReader(filepath); bool even = true; int x = 0; int y = 0; while (sLine != null) { sLine = objReader.ReadLine(); if (sLine != null && sLine != "") { if (sLine == "new") { pointsToCheck.Add(listOfCoords); listOfCoords = new List<Vector2D>(); even = true; } else { if (even) { x = Convert.ToInt32(sLine); even = false; } else { y = Convert.ToInt32(sLine); Vector2D point = new Vector2D(x, y); listOfCoords.Add(point); even = true; } } } } // convert to millimetres foreach (List<Vector2D> l in pointsToCheck) foreach (Vector2D v in l) v.scale(10.0); objReader.Close(); return pointsToCheck; }
public void updateRbotPosition(double x, double y, double rbotAngle) { Vector2D v = new Vector2D(x*UNIT, y*UNIT); this.rbotPos = v; this.rbotAngle = rbotAngle; Console.WriteLine(); Console.WriteLine("Robot Position/Angle updated @ " + System.DateTime.Now + " to (" + x + ", " + y + "; " + rbotAngle + ")"); }
/// <summary> /// Event handler for Kinect sensor's DepthFrameReady event /// </summary> /// <param name="sender">object sending the event</param> /// <param name="e">event arguments</param> private void SensorDepthFrameReady(object sender, DepthImageFrameReadyEventArgs e) { using (DepthImageFrame depthFrame = e.OpenDepthImageFrame()) { // Check lock, check rbot position is known, and check frame count if (readyToRead && rbotPos!=null && framesRead < framesPerRead) { if (depthFrame != null) { // Copy the pixel data from the image to a temporary array depthFrame.CopyDepthImagePixelDataTo(this.depthPixels); // Lists of vectors of free points in space and occupied points in space List<Vector2D> freeVectors = new List<Vector2D>(); List<Vector2D> occupiedVectors = new List<Vector2D>(); // Frame dimensions int width = 640; int height = 480; // Analyse the middle line int start = width * (height / 2 - 1); // Go through each pixel of the middle line for (int i = 0 + MARGIN; i < width - MARGIN; ++i) { // Get the depth corresponding to this pixel from the Kinect [in millimiters] short depth = this.depthPixels[i + start].Depth; double anglePerPixel = 58.0 / 640.0; double angle = DegreesToRadians(119 - i * anglePerPixel); // Project to obtain (x,y) coordinates relative to kinect double xFromRBot = Math.Cos(angle) * depth; double yFromRBot = Math.Sin(angle) * depth; Vector2D posFromRBot = new Vector2D(xFromRBot, yFromRBot); // Adjust for kinect's sensor reading in flipped view posFromRBot.reflectInYaxis(); // Check range and add obstacle if (1200 <= depth && depth <= 3000) occupiedVectors.Add(posFromRBot); // If obstacle within augmented accepted depth interval, then add free "line of sight" if (1200 <= depth && depth <= 6000) { for (int k = 0; k < pointsToCheck[i].Count; ++k) { Vector2D v = pointsToCheck[i][k]; if (v.getModulusSquared() < depth * depth) freeVectors.Add(new Vector2D((-1.0)*v.getX(), v.getY())); } } } // Collate list of observations to send, transformed to global coordinates List<Observation> observations = new List<Observation>(); foreach (Vector2D v in freeVectors) observations.Add(Observation.vectorToObservation(v, false, rbotPos, rbotAngle, UNIT, 127)); foreach (Vector2D v in occupiedVectors) observations.Add(Observation.vectorToObservation(v, true, rbotPos, rbotAngle, UNIT, 127)); Console.WriteLine(); Console.WriteLine("Observation set is generated @ " + System.DateTime.Now); // Send observations foreach (Observation o in observations) server.SendPacket(o.serialize()); Console.WriteLine("Observation set packets sent @ " + System.DateTime.Now); framesRead++; if (framesRead == framesPerRead) server.SendPacket(ServerCommands.getRUnlockMessage()); } } } }
public void translate(Vector2D v) { x = x + v.getX(); y = y + v.getY(); }