//public delegate void ObjectSizeDetectedEvent(object sender, ObjectSizeDetectedArgs e);
public ObstaclesOverlay(IObstacleDetector obstacleDetector) : base()
{
this.Name = "Obstacles";
_detectedObstacles = null;
obstacleDetector.ObstaclesDetected += obstacleDetector_ObstaclesDetected;
}
public MarsRover(Point startingPoint, CardinalDirection startingDirection, Grid map = null, IObstacleDetector obstacleDetector = null)
{
this._map = map ?? new Grid();
this.ObstacleDetector = obstacleDetector;
Status = new RoverStatus();
var normalizedStartingPoint = this._map.NormalizeCoordinates(startingPoint);
this.Position = new RoverPosition(normalizedStartingPoint, startingDirection);
}
public RoverEngine(int x, int y, EDirection direction, int gridHeight, int gridWidth, IObstacleDetector obstacleDetector)
{
_x = x;
_y = y;
_gridHeight = gridHeight;
_gridWidth = gridWidth;
_direction = direction;
_obstacleDetector = obstacleDetector ?? throw new ArgumentNullException(nameof(obstacleDetector));
}
//****************** 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; // ?
}
public GoCommandFactory(RoverState roverState, INavigator navigator, IObstacleDetector obstacleDetector)
{
RoverState = roverState;
Navigator = navigator;
ObstacleDetector = obstacleDetector;
}
protected MoveCommand(RoverState roverState, INavigator navigator, IObstacleDetector obstacleDetector)
: base(roverState)
{
Navigator = navigator;
ObstacleDetector = obstacleDetector;
}
public MoveForwardCommand(RoverState roverState, INavigator navigator, IObstacleDetector obstacleDetector)
: base(roverState, navigator, obstacleDetector)
{
}
public Rover(Position position, Direction direction, IGrid grid)
{
RoverState = new RoverState(position, direction);
_navigator = new Navigator(grid);
_obstacleDetector = new ObstacleDetector(grid);
}
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();
}
