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