// Returns the grid points in XY occupied by the given obstacle
public List <mPoint> LocalizeObstacle(Obstacle obstacle)
{
double currentDirection = AngleUtils.DegreesToRadians(CurrentPose.CurrentDirection);
// Convert the left and right points from mm to cm
Vector leftObstaclePoint = obstacle.leftPoint / 10;
Vector rightObstaclePoint = obstacle.rightPoint / 10;
// Find the offset of the left and right of the obstacle from the current position
Vector leftOffsetVector = CurrentPose.CurrentDirectionVector + VectorUtils.RotateVector(leftObstaclePoint, currentDirection);
Vector rightOffsetVector = CurrentPose.CurrentDirectionVector + VectorUtils.RotateVector(rightObstaclePoint, currentDirection);
// Estimate the locations of the actual grid points
mPoint leftMapPoint = new mPoint(CurrentPose.CurrentPositionInXY.X + leftOffsetVector.X, CurrentPose.CurrentPositionInXY.Y + leftOffsetVector.Y, CurrentPose.CurrentRoom);
mPoint rightMapPoint = new mPoint(CurrentPose.CurrentPositionInXY.X + leftOffsetVector.X, CurrentPose.CurrentPositionInXY.Y + leftOffsetVector.Y, CurrentPose.CurrentRoom);
// Find the actual grid points from the estimated grid points
leftMapPoint = CurrentPose.CurrentRoom.GetClosestGridPoint(leftMapPoint.GetUVFromXY(CurrentPose.CurrentRoom));
rightMapPoint = CurrentPose.CurrentRoom.GetClosestGridPoint(rightMapPoint.GetUVFromXY(CurrentPose.CurrentRoom));
List <mPoint> points = new List <mPoint>();
points.Add(leftMapPoint);
points.Add(rightMapPoint);
return(points);
}
private void MapImage_MouseMove(object sender, System.Windows.Input.MouseEventArgs e)
{
Point click = e.GetPosition(MapImage);
mPoint clickPoint = new mPoint(click.X, click.Y);
Tuple <mPoint, Tuple <double, double> > clickRegion = Data.Cartographer.GetRoomCornerAndDimentions(clickPoint);
highlightedRooms.Children.Clear();
if (clickRegion != null)
{
currentSelectedRoom.Width = clickRegion.Item2.Item1;
currentSelectedRoom.Height = clickRegion.Item2.Item2;
SolidColorBrush solidColorBrush = new SolidColorBrush();
var r = (byte)0;
var g = (byte)255;
var b = (byte)0;
var a = (byte)50;
solidColorBrush.Color = System.Windows.Media.Color.FromArgb(a, r, g, b);
currentSelectedRoom.Fill = solidColorBrush;
Canvas.SetLeft(currentSelectedRoom, clickRegion.Item1.X);
Canvas.SetTop(currentSelectedRoom, clickRegion.Item1.Y);
highlightedRooms.Children.Add(currentSelectedRoom);
}
}
//****************** Internal Methods ******************
//looks at the first element of the route and passes it to Cartographer. This will return a vector to get us to that location
//From there we look at the difference between that vector and our direction vector and calculate the difference in degrees
//we use this to adjust the desired direction vector to account for our faceing. From there we normalize it and return the vector.
private Vector NavigateFromMap()
{
lock (Route)
{
mPoint currentPosition = CurrentPose.CurrentPositionInUV;
Vector currentDirection = CurrentPose.CurrentDirectionVector;
//if we dont have a route to follow or if the path is empty stop
if (Route.Count == 0 || Path.Count == 0)
{
return(new Vector(0, 0));
}
Vector desiredDirection = Cartographer.VectorTo(currentPosition, Path[0]);
//get the dif in radians between the current direction vector and the desired direction
double difAngleInRadians = DifInRaidans(desiredDirection, currentDirection);
// Correct the target angle by 90 degrees
// Unsure why so far, issue #18 is tracking this
//difAngleInRadians -= Math.PI;
// Convert difAngleInRadians to a unit vector
return(VectorUtils.VectorFromAngle(difAngleInRadians));
}
}
public void SetRoute(mPoint nextPointUV)
{
lock (Route)
{
Route = new List <mPoint>();
Route.Add(CurrentPose.CurrentPositionInUV);
Route.Add(nextPointUV);
CurrentDriveType = DriveTypeStatus.RoutDriven;
}
}
private void LetsGo_Click(object sender, RoutedEventArgs e)
{
string us = U.Text;
string vs = V.Text;
double u; double v;
Double.TryParse(us, out u);
Double.TryParse(vs, out v);
mPoint clickPoint = new mPoint(u, v);
//Data.Navigator.NavigateTo(clickPoint);
Data.Navigator.SetRoute(clickPoint);
}
/// <summary>
/// Signals the navigator to find a route to the given name destination.
/// sets the driveType to driven by route unless the rout was null which
/// means there was an error in cartographer and you cant get to that point
/// </summary>
/// <param name="destination">Name of location to route to.</param>
public void NavigateTo(mPoint destinationPointInUV)
{
lock (Route)
{
mPoint currentPosition = CurrentPose.CurrentPositionInUV;
List <mPoint> newRoute = Cartographer.PlanRoute(currentPosition, destinationPointInUV);
//if we have a valid route set the route and switch to route driven else set to user driven
if (newRoute != null)
{
Route = newRoute;
Path = new List <mPoint>();
CurrentDriveType = DriveTypeStatus.RoutDriven;
}
else
{
SetToUserDriven();
}
}
}
//****************** Consturtors ******************
public Navigator(IObstacleMap obstacleMap, IObstacleDetector obstacleDetector, ObstacleLocalizer obstacleLocalizer,
ICartographer cartographer, IDoorDetector doorDetector, Pose pose, SonarDetector sonarDetector,
SensorArray sensorArray)
{
this.ObstacleDetector = obstacleDetector;
this.ObstacleMap = obstacleMap;
this.ObstacleLocalizer = obstacleLocalizer;
this.DoorDetector = doorDetector;
this.CurrentPose = pose;
this.Cartographer = cartographer;
this.SonarDetector = sonarDetector;
this.ChairSensorArray = sensorArray;
pose.CurrentRoom = Cartographer.GetStartingRoom();
LastPoint = pose.CurrentPositionInUV;
FollowRouteThread = new Thread(UpdateRoute);
FollowRouteThread.Name = "FollowRouteThread";
FollowRouteThread.Start();
this.ObstacleDetector.ObstaclesDetected += ObstacleDetector_ObstaclesDetected; // ?
}
//****************** Methods for Threads ******************
/// <summary>
/// Updates the current route, poppin the next destination off the
/// stack if we've arrived at it. If we dont have a route to follow
/// it will just update the current position.
/// </summary>
/// ToDo: might need to update to allow the chair to recover from
/// door navigation correctly
private void UpdateRoute()
{
while (true)
{
//update the position and grab it for use.
mPoint mostCurrentPosition = CurrentPose.CurrentPositionInUV;
lock (Route)
{
//if we have a path to follow.
if (Path.Count > 0)
{
double pathDestinationDif = Path[0].GetDistanceToPoint(mostCurrentPosition);
//if the point are checking is a door then use the DistanceToBeAtDoor else use DistanceToBeAtPoint
if (Path[0].IsDoor() && pathDestinationDif < DistanceToBeAtDoor)
{
Path.RemoveAt(0);
}
else if (pathDestinationDif < DistanceToBeAtPoint)
{
Path.RemoveAt(0);
}
}
//Check if I'm at my next destination and if so, pop it off the stack.
//if we have a rout we need to follow
if (Route.Count > 0)
{
//get the distance from the current location to the next location
double distanceToDestination = Route[0].GetDistanceToPoint(mostCurrentPosition);
//if our destination is a door and we are there
if (Route[0].IsDoor() && distanceToDestination < DistanceToBeAtDoor)
{
//swap to door driven
InitializeDoorNavigation();
CurrentDriveType = DriveTypeStatus.DoorDriven;
Route.RemoveAt(0);
}
// if we are at the destination
else if (distanceToDestination < DistanceToBeAtPoint)
{
Route.RemoveAt(0);
}
}
//if we have a route but not a path setup the new path and we are still in routeDriven mode.
//Note: the RoutDriven check makes sure we are not trying to get through a door and setting up a path for it
if ((Route.Count > 0 && Path.Count == 0) && CurrentDriveType == DriveTypeStatus.RoutDriven)
{
List <mPoint> newPath = Cartographer.PlanPath(mostCurrentPosition, Route[0], this.ObstacleMap);
//if we can get to the new locating set up the path to do so else set us back to UserDriven
if (newPath != null)
{
Path = newPath;
}
else
{
SetToUserDriven();
}
}
}
Thread.Sleep(500);
}
}
/// <summary>
/// This is the core algorithm to fill the depressions in an image
/// This concept is masterminded by the self proclaimed genius Ted Dunsford
/// and is being prepared for publication.
/// </summary>
/// <param name="mwSourceGrid">The MapWinGIS.Grid to read the actual elevation values from</param>
/// <param name="mwDestGrid">The MapWinGIS.Grid to write the elevations to</param>
/// <param name="SpawnPoints">A list of points on the edges where the algorithm will start </param>
/// <param name="myFrame">The current sub-division of the overall grid</param>
/// <param name="DepsExist">A list of booleans for each direction indicating if dependencies were found in neighboring frames</param>
/// <param name="myProgress">A Dialogs.ProgressDialog indicating the current progress of the algorithm.</param>
/// <returns>TimeSpan indicating the time the function took to run the algorithm</returns>
public static System.TimeSpan FloodDependencies(MapWinGIS.Grid mwSourceGrid, MapWinGIS.Grid mwDestGrid, List <Frame.Loc> SpawnPoints, Frame myFrame, bool[] DepsExist, Dialogs.ProgressDialog myProgress)
{
string strSpawnPoints = "null";
string strFrame = "null";
int Prog = 0;
int OldProg = 0;
if (SpawnPoints != null)
{
strSpawnPoints = SpawnPoints.Count.ToString();
}
if (myFrame != null)
{
strFrame = "Frame [" + myFrame.Width.ToString() + " x " + myFrame.Height.ToString() + "]";
}
MapWinUtility.Logger.Dbg("FloodDependencies(mwSourceGrid: " + Macro.ParamName(mwSourceGrid) + ",\n" +
" mwDestGrid: " + Macro.ParamName(mwDestGrid) + ",\n" +
" SpawnPoints: " + strSpawnPoints + " Locations,\n" +
" myFrame: " + strSpawnPoints + ",\n" +
" DepsExist: " + DepsExist.ToString() +
" ProgressDialog)");
if (mwSourceGrid == null)
{
MapWinUtility.Logger.Dbg("Argument Exception: mwSourceGrid cannot be null.");
throw new ArgumentException("mwSourceGrid cannot be null.");
}
if (mwDestGrid == null)
{
MapWinUtility.Logger.Dbg("Argument Exception: mwDestGrid cannot be null.");
throw new ArgumentException("mwDestGrid cannot be null.");
}
if (SpawnPoints == null)
{
MapWinUtility.Logger.Dbg("Argument Exception: SpawnPoints cannot be null.");
throw new ArgumentException("SpawnPoints cannot be null.");
}
if (myFrame == null)
{
MapWinUtility.Logger.Dbg("Argument Exception: myFrame cannot be null.");
throw new ArgumentException("myFrame cannot be null.");
}
System.Drawing.Rectangle Window = myFrame.Rectangle;
float[][] Source;
float[][] Dest;
int numRows = Window.Height;
int numCols = Window.Width;
mPoint pt;
Frame.Loc ptStart;
List <mPoint> DryCells = new List <mPoint>();
List <mPoint> WetCells = new List <mPoint>();
float[] Top = new float[numCols];
float[] Bottom = new float[numCols];
float[] Left = new float[numRows];
float[] Right = new float[numRows];
// Since we are evaluating this frame, we can clear its dependencies
myFrame.Set_HasDependencies(UP, false);
myFrame.Set_HasDependencies(DOWN, false);
myFrame.Set_HasDependencies(LEFT, false);
myFrame.Set_HasDependencies(RIGHT, false);
myProgress.WriteMessage("Loading File...");
MapWinUtility.Logger.Progress("Loading File...", Prog, OldProg);
Source = myFrame.GetArrayFromWindow(mwSourceGrid, myProgress.ICallBack);
// Initialize first time around. We will be writing over the "copy" with
// any changes we make, but this should save some time in file handling
if (myFrame.Parent.First_Time(myFrame.X, myFrame.Y) == true)
{
if (myFrame.Parent.numCols * myFrame.Parent.numRows > 64000000)
{
// in the case where we copied the file, we need the middle cells to be set
// to the float maximum for the algorithm to work.
Dest = myFrame.GetBorders(mwSourceGrid, mwDestGrid, myProgress.ICallBack);
myFrame.Parent.set_First_Time(myFrame.X, myFrame.Y, false);
}
else
{
Dest = myFrame.GetArrayFromWindow(mwDestGrid, myProgress.ICallBack);
}
}
else
{
Dest = myFrame.GetArrayFromWindow(mwDestGrid, myProgress.ICallBack);
}
DateTime AlgStart = new DateTime();
AlgStart = DateTime.Now;
for (int iPoint = 0; iPoint < SpawnPoints.Count; iPoint++)
{
ptStart = SpawnPoints[iPoint];
pt = new mPoint(ptStart.Y, ptStart.X, Dest[ptStart.Y][ptStart.X]);
if (Dest[ptStart.Y][ptStart.X] == Source[ptStart.Y][ptStart.X])
{
DryCells.Add(pt);
}
else
{
WetCells.Add(pt);
}
}
// Even though we have specific dependency locations for our source, we can potentially
// effect any other cells on the perimeter
for (int col = 0; col < numCols; col++)
{
Top[col] = Dest[0][col];
Bottom[col] = Dest[numRows - 1][col];
}
for (int row = 0; row < numRows; row++)
{
Left[row] = Dest[row][0];
Right[row] = Dest[row][numCols - 1];
}
if (DryCells.Count > 0)
{
WetCells.AddRange(DryUpward(Source, ref Dest, numRows, numCols, DryCells, myProgress.ICallBack));
}
FloodFill(Source, ref Dest, numRows, numCols, WetCells, myProgress.ICallBack);
TimeSpan AlgTime = DateTime.Now - AlgStart;
myProgress.WriteMessage("Saving Array to File...");
MapWinUtility.Logger.Progress("Saving Array to File...", Prog, OldProg);
myFrame.SaveArrayToWindow(Dest, mwDestGrid, myProgress.ICallBack);
myProgress.WriteMessage("Done.\n");
MapWinUtility.Logger.Progress("", Prog, OldProg);
byte[][] Dependencies = new byte[5][];
Dependencies[UP] = new byte[numCols];
Dependencies[DOWN] = new byte[numCols];
DepsExist[UP] = false;
DepsExist[DOWN] = false;
for (int col = 0; col < numCols; col++)
{
if (Top[col] != Dest[0][col])
{
DepsExist[UP] = true;
Dependencies[UP][col] = (byte)DOWN;
//if (Dest[0][col] < Minima[UP]) Minima[UP] = Dest[0][col];
}
else
{
Dependencies[UP][col] = (byte)0;
}
if (Bottom[col] != Dest[numRows - 1][col])
{
DepsExist[DOWN] = true;
Dependencies[DOWN][col] = (byte)UP;
//if (Dest[numRows - 1][col] < Minima[DOWN]) Minima[DOWN] = Dest[numRows - 1][col];
}
else
{
Dependencies[DOWN][col] = (byte)0;
}
}
// Save Dependencies to the files
myFrame.Write_Dependencies(Dependencies[UP], UP);
myFrame.Write_Dependencies(Dependencies[DOWN], DOWN);
Dependencies[LEFT] = new byte[numRows];
Dependencies[RIGHT] = new byte[numRows];
DepsExist[LEFT] = false;
DepsExist[RIGHT] = false;
for (int row = 0; row < numRows; row++)
{
if (Left[row] != Dest[row][0])
{
DepsExist[LEFT] = true;
Dependencies[LEFT][row] = (byte)RIGHT;
//if (Dest[row][0] < Minima[LEFT]) Minima[LEFT] = Dest[row][0];
}
else
{
Dependencies[LEFT][row] = (byte)0;
}
if (Right[row] != Dest[row][numCols - 1])
{
DepsExist[RIGHT] = true;
Dependencies[RIGHT][row] = (byte)LEFT;
//if (Dest[row][numCols - 1] < Minima[RIGHT]) Minima[RIGHT] = Dest[row][numCols - 1];
}
else
{
Dependencies[RIGHT][row] = (byte)0;
}
}
// Save Vertical Dependencies to the file
myFrame.Write_Dependencies(Dependencies[LEFT], LEFT);
myFrame.Write_Dependencies(Dependencies[RIGHT], RIGHT);
// Update the status of the neighbors
myFrame.Update_Neighbor_Status(DepsExist);
MapWinUtility.Logger.Dbg("Finished FloodDependencies");
return(AlgTime);
}
public void SetRoute(mPoint nextPointUV)
{
throw new NotImplementedException();
}
public void NavigateTo(mPoint destinationPointInUV)
{
throw new NotImplementedException();
}
public void DrawMap()
{
const DispatcherPriority priority = DispatcherPriority.Render;
while (true)
{
if (Data.Cartographer != null)
{
MapImage.Dispatcher.Invoke(() =>
{
Tuple <double, double> tupleUV = new Tuple <double, double>(0, 0);
mPoint myUV = new mPoint(tupleUV);
myUV = this.CurrentPose.CurrentPositionInUV;
if (myUV.X < MapImage.Width - 10 && myUV.Y < MapImage.Height - 10)
{
SolidColorBrush solidColorBrush1 = new SolidColorBrush();
SetMapDotColor(Data.Navigator.GetCurrentDriveType(), solidColorBrush1);
Ellipse dot = new Ellipse();
dot.Width = 10;
dot.Height = 10;
dot.Fill = solidColorBrush1;
Canvas.SetLeft(dot, myUV.X);
Canvas.SetTop(dot, myUV.Y);
recentLocations.Children.Add(dot);
SolidColorBrush solidColorBrush2 = new SolidColorBrush();
}
if (recentLocations.Children.Count > 5)
{
recentLocations.Children.RemoveAt(0);
}
if (obstacleCanvas.Children.Count > 3)
{
obstacleCanvas.Children.RemoveAt(0);
}
}, priority);
roomLabel.Dispatcher.Invoke(() =>
{
roomLabel.Content = "Room Location: " + this.CurrentPose.CurrentRoom.Name;
});
drivetypeLabel.Dispatcher.Invoke(() =>
{
if (drive_type == DriveType.User)
{
drivetypeLabel.Content = "Drive Type: User Driven";
}
else if (drive_type == DriveType.Override_user)
{
drivetypeLabel.Content = "Drive Type: User Override Driven";
}
else if (drive_type == DriveType.Route)
{
drivetypeLabel.Content = "Drive Type: Route Driven";
}
else if (drive_type == DriveType.Door)
{
drivetypeLabel.Content = "Drive Type: Door Detected Driven";
}
else
{
drivetypeLabel.Content = "Drive Type: Error Detected";
}
});
ucoord.Dispatcher.Invoke(() =>
{
mPoint myUV = CurrentPose.CurrentPositionInUV;
mPoint myXY = CurrentPose.CurrentPositionInXY;
ucoord.Content = " Current Location U/X: " + myUV.X + " / " + myXY.X;
});
vcoord.Dispatcher.Invoke(() =>
{
mPoint myUV = myUV = CurrentPose.CurrentPositionInUV;
mPoint myXY = CurrentPose.CurrentPositionInXY;
vcoord.Content = " Current Location V/Y: " + myUV.Y + " / " + myXY.Y;
});
compassLabel.Dispatcher.Invoke(() =>
{
double degrees = CurrentPose.CurrentDirection;
compassLabel.Content = "Direction: " + degrees.ToString();
});
SonarLabel.Dispatcher.Invoke(() =>
{
int distance1 = Data.Navigator.getDistanceFromSonar(0);
int distance2 = Data.Navigator.getDistanceFromSonar(1);
int distance3 = Data.Navigator.getDistanceFromSonar(2);
int distance4 = Data.Navigator.getDistanceFromSonar(3);
int distance5 = Data.Navigator.getDistanceFromSonar(4);
SonarLabel.Content = "S1:" + distance1 + ";S2:" + distance2 + ";S3:" + distance3 + ";S4:" + distance4 + ";S5:" + distance5;
});
}
Thread.Sleep((int)(100 / FRAMERATE));
}
}
private void MapImage_MouseLeftButtonDown(object sender, System.Windows.Input.MouseButtonEventArgs e)
{
Point click = e.GetPosition(MapImage);
mPoint clickPoint = new mPoint(click.X, click.Y);
Tuple <mPoint, Tuple <double, double> > clickRegion = Data.Cartographer.GetRoomCornerAndDimentions(clickPoint);
pathLocations.Children.Clear();
routeLocations.Children.Clear();
Thread.Sleep(500);
highlightedRooms.Children.Clear();
if (clickRegion != null)
{
currentSelectedRoom.Width = clickRegion.Item2.Item1;
currentSelectedRoom.Height = clickRegion.Item2.Item2;
SolidColorBrush solidColorBrush = new SolidColorBrush();
var r = (byte)0;
var g = (byte)255;
var b = (byte)0;
var a = (byte)50;
// solidColorBrush.Color = System.Windows.Media.Color.FromArgb(a, r, g, b);
solidColorBrush.Color = System.Windows.Media.Colors.LightBlue;
currentSelectedRoom.Fill = solidColorBrush;
Canvas.SetLeft(currentSelectedRoom, clickRegion.Item1.X);
Canvas.SetTop(currentSelectedRoom, clickRegion.Item1.Y);
highlightedRooms.Children.Add(currentSelectedRoom);
}
Data.Navigator.NavigateTo(clickPoint);
if (Data.Navigator.GetCurrentDriveType() == 2)
{
//SetMapDotColor(Data.Navigator.GetCurrentDriveType(), solidColorBrush);
Thread.Sleep(500);
List <mPoint> pathList = Data.Navigator.GetPath();
List <mPoint> routeList = Data.Navigator.GetRoute();
foreach (mPoint rl in routeList)
{
Ellipse dot = new Ellipse();
dot.Width = 10;
dot.Height = 10;
dot.Fill = Brushes.Magenta;
Canvas.SetLeft(dot, rl.X);
Canvas.SetTop(dot, rl.Y);
routeLocations.Children.Add(dot);
}
foreach (mPoint pl in pathList)
{
Ellipse dot = new Ellipse();
dot.Width = 10;
dot.Height = 10;
dot.Fill = Brushes.MediumPurple;
Canvas.SetLeft(dot, pl.X);
Canvas.SetTop(dot, pl.Y);
pathLocations.Children.Add(dot);
}
}
}
static void Main()
{
//change the test file xml here and make sure it is in bin.
string startingXMLFile = "ETRL_03_07.xml";
//string startingXMLFile = "FloorPlanTest.xml";
//string startingOpenFile = "Sloan46_FINAL.xml";
double compassToMapOffset = 72.0;
// List of all the objects we'll be initializing.
// Default to null, as some of them will only
// be initialized conditionally e.g.
// vehicle is only initialized if
// there is a vehicle connected.
SerialPort sp = null;
IVehicle vehicle = null;
IImageStream depthStream = null;
IImageStream videoStream = null;
IObstacleDetector obstacleDetector = null;
ICartographer cartographer = null;
IOdometer odometer = null;
IInputDevice input = KeyboardInput.Instance;
ObstacleMap obstacleMap = null;
INavigator navigator = null;
MainWindow mainWindow = null;
Driver.Driver driver = null;
VisualInputGridPresenter uiInputPresenter = null;
ObstacleGridPresenter uiGridPresenter = null;
PositioningSystem ips = null;
OverlayRenderer overlayRenderer = null;
SensorArray sensorArray = null;
Pose pose = null;
IDoorDetector doorDetector = null;
ObstacleLocalizer obstacleLocalizer = null;
SonarDetector sonarDetector = null;
Config.Initialize();
Devices.Initialize();
//string wheelchair_com_port = Devices.IsWheelchairConnected();
string wheelchair_com_port = "COM3";//Devices.FindComPort("Arduino Uno");
bool WheelChairConnected = false;
if (wheelchair_com_port != "")
{
WheelChairConnected = true;
}
//bool WheelChairConnected = true;
bool EyeTribeConnected = true; // Devices.IsEyeTribeConnected();
bool ControllerConnected = true; //Devices.IsControllerConnected();
bool KinectConnected = true; //Devices.IsKinectConnected();
//Console.WriteLine("Kinect Connected: {0}", KinectConnected.ToString());
//Console.WriteLine("Eyetribe Connected: {0}", EyeTribeConnected.ToString());
//Console.WriteLine("Wheelchair Connected: {0}", WheelChairConnected.ToString());
//Console.WriteLine("Controller Connected: {0}", ControllerConnected.ToString());
// Initialize vehicle and serial connection if
// there is a vehicle connected.
if (WheelChairConnected)
{
//sp = new SerialPort(wheelchair_com_port, 9600);
vehicle = Wheelchair.Instance(wheelchair_com_port);
vehicle.initializeOffset(Properties.Settings.Default.CalibrationOffset);
}
else
{
vehicle = new MockVehicle();
MockVehicleWindow mockDisplay = new MockVehicleWindow((MockVehicle)vehicle);
mockDisplay.Show();
}
//initalize IPS here
IByteReader sensorReader = null;
try
{
//sensorReader = new SerialByteReader("Arduino Mega");
sensorReader = new SerialByteReader("COM4");
}
catch (Exception e)
{
MessageBox.Show(e.Message);
}
if (sensorReader != null)
{
ILogger logger = new NullLogger();
sensorArray = new SensorArray(sensorReader, logger);
sonarDetector = new SonarDetector();
}
else
{
sensorReader = new NullByteReader();
ILogger logger = new NullLogger();
sensorArray = new SensorArray(sensorReader, logger);
}
IByteReader byteReader = null;
try
{
//byteReader = new SerialByteReader(Devices.FindComPort("STMicroelectronics"));
byteReader = new SerialByteReader("COM3");
}
catch (Exception e)
{
MessageBox.Show(e.Message);
}
if (byteReader != null)
{
ILogger logger = new NullLogger();
ips = new MarvelMind(byteReader, logger);
}
else
{
//Setup Mock IPS
//IByteReader mockByteReader = new FileByteReader(@"C:\Users\Dana\Documents\Visual Studio 2013\Projects\UITests\UITests\Mock_IPS_Data.txt");
//IByteReader mockByteReader = new XLineFixedYByteReader(800, 100, 200);
IByteReader mockByteReader = new RandomByteReader(300, 299);
ILogger mockLogger = new NullLogger();
ips = new MarvelMind(mockByteReader, mockLogger);
}
//wait for an iteration of the IPS system that way
//we can get the starting location
while (false == ips.XUpdated && false == ips.YUpdated)
{
continue;
}
//Tuple<double, double> t = new Tuple<double, double>((double)ips.X, (double)ips.Y);
Tuple <double, double> t = new Tuple <double, double>((double)ips.Y, (double)ips.X);
mPoint startingLocationInXY = new mPoint(t);
// UI, input, object detection, navigation, and driver are initialized always,
// as they are not directly dependent on external hardware.
obstacleMap = new ObstacleMap();
//cartographer = new Cartographer(Directory.GetCurrentDirectory() + @"\FloorPlanTest.xml", ips);
// might need to be changed later. If the location is not in the starting room. add staring room.
//cartographer = new Cartographer(Directory.GetCurrentDirectory() + @"\FloorPlanTest.xml", startingLocationInXY);
OpenFileDialog ofd = new OpenFileDialog();
ofd.Filter = "XML | *.xml";
ofd.Title = "Open Map";
if (ofd.ShowDialog() == true)
{
startingXMLFile = ofd.FileName;
Console.WriteLine(startingXMLFile);
}
cartographer = new Cartographer(startingXMLFile, startingLocationInXY);
pose = new Pose(ips, cartographer.GetStartingRoom(), sensorArray.CompassDevice, compassToMapOffset);
overlayRenderer = new OverlayRenderer();
if (KinectConnected)
{
depthStream = DepthStream.Instance;
videoStream = VideoStream.Instance;
// Initialize depthstream if kinect is connected.
obstacleDetector = new ObstacleDetector(depthStream, false);
obstacleDetector.Start();
obstacleLocalizer = new ObstacleLocalizer(pose);
ObstaclesOverlay obstaclesOverlay = new ObstaclesOverlay(obstacleDetector);
overlayRenderer.Overlays.Add(obstaclesOverlay);
doorDetector = new DepthBasedDoorDetector();
doorDetector.RunAsync(depthStream);
try
{
sonarDetector.RunAsync(sensorArray);
}
catch (Exception e)
{
}
}
// Obstacle detector and driver are only run
// if both the vehicle and kinect are connected.
if (vehicle != null && KinectConnected)
{
Vector startPoint = new Vector(7 / 0.75f, 43 / 0.75f);
Vector startRotation = new Vector(-7, -43);
startRotation.Normalize();
odometer = new Odometer();
navigator = new Navigator(obstacleMap, obstacleDetector, obstacleLocalizer, cartographer, doorDetector, pose, sonarDetector, sensorArray);
driver = new Driver.Driver(input, vehicle, navigator);
driver.Start();
}
mainWindow = new MainWindow(pose, obstacleMap, cartographer, navigator, doorDetector, overlayRenderer, EyeTribeConnected, ControllerConnected, sensorArray);
uiInputPresenter = new VisualInputGridPresenter(mainWindow.visualInputGrid);
//This starts the program.
Application app = new Application();
app.ShutdownMode = ShutdownMode.OnMainWindowClose;
app.MainWindow = mainWindow;
app.MainWindow.Show();
app.Run();
}
