protected override void Initialize()
{
graphics.PreferMultiSampling = true;
graphics.PreferredBackBufferWidth = WINDOWED_WIDTH;
graphics.PreferredBackBufferHeight = WINDOWED_HEIGHT;
graphics.IsFullScreen = false;
graphics.ApplyChanges();
spriteBatch = new SpriteBatch(GraphicsDevice);
console = new GameConsole(this, spriteBatch, new GameConsoleOptions()
{
OpenOnWrite = false,
Prompt = ">",
Height = 500,
ToggleKey = Keys.Escape
});
console.AddCommand("load",
args =>
{
missionFile = "../../../../../missions/" + args[0] + ".json";
ClearWorld();
InitSim(false);
return "Mission loaded.";
}, "Load a mission file");
console.AddCommand("experiment",
args =>
{
if (args[0] == "random")
new SmootherExperiment(this, expFile, 100);
else if (args[0] == "parking")
new ParkingExperiment(this, world, expFile);
else
return "Experiment not found.";
return "Experiment started.";
}, "Run an experiment");
console.AddCommand("antialias",
args =>
{
graphics.PreferMultiSampling = !graphics.PreferMultiSampling;
graphics.ApplyChanges();
return "Antialias " + (graphics.PreferMultiSampling ? "on" : "off");
}, "Toggles antialias");
console.AddCommand("debug",
args =>
{
showDebugInfo = !showDebugInfo;
return "Debug " + (showDebugInfo ? "on" : "off");
}, "Toggles the debug info");
console.AddCommand("dashboard",
args =>
{
showDashboard = !showDashboard;
return "Dashboard " + (showDashboard ? "on" : "off");
}, "Toggles the dashboard");
console.AddCommand("param",
args =>
{
if (args.Length < 1) return "";
switch (args[0].ToLower())
{
case "smoother":
return Smoother.HandleParamCommand(args);
default:
return "";
}
}, "Prints or updates parameters");
console.AddCommand("smooth",
args =>
{
if (pathSmoothDone)
{
bg = new Thread(() =>
{
DateTime now = DateTime.Now;
smoothedPath = Smoother.Smooth(astar.Path, grid);
int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0;
console.WriteLine("Smoothing Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)");
controller = new StanleyFSMController(smoothedPath, goalPose);
});
bg.IsBackground = true;
bg.Priority = ThreadPriority.Lowest;
bg.Start();
}
return "";
}, "Smooths the path using the current parameters");
console.AddCommand("smoothing",
args =>
{
smoothing = !smoothing;
return "Smoothing " + (smoothing ? "on" : "off");
}, "Toggles smoothing"
);
dashboard = new Dashboard(this);
world = new World(Vector2.Zero);
car = new Car(world, new Pose());
camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f);
camera.Position = new Vector3(75f, 75f, 180f);
base.Initialize();
}
protected override void Initialize()
{
graphics.PreferMultiSampling = true;
graphics.PreferredBackBufferWidth = WINDOWED_WIDTH;
graphics.PreferredBackBufferHeight = WINDOWED_HEIGHT;
graphics.IsFullScreen = false;
graphics.ApplyChanges();
spriteBatch = new SpriteBatch(GraphicsDevice);
camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f);
camera.Position = new Vector3(0f, 0f, 140f);
world = new World(Vector2.Zero);
numCells = (cells + 1) * (cells + 1);
cellSize = square / cells;
calcRS();
base.Initialize();
}
public void InitSim(bool autoStart, bool smoothingOn, Mission mission)
{
gridDone = false;
pathDone = false;
pathSmoothDone = false;
pathSearching = false;
pathSearchingDone = false;
autoDrive = true;
run = false;
isCollided = false;
currentControls = new CarControls(0f, 0f, 0f);
if (camera == null)
{
camera = new Camera(MathHelper.PiOver4, GraphicsDevice.Viewport.AspectRatio, 0.1f, 1000f);
camera.Position = new Vector3(75f, 75f, 180f);
}
startPose = mission.Start;
goalPose = mission.Goal;
HybridAStar.Epsilon = mission.AStarEpsilon;
HybridAStar.GridResolution = mission.AStarGridResolution;
HybridAStar.SafetyFactor = 1.5f;
HybridAStar.Reset();
car = new Car(world, startPose);
grid = new ObstacleGrid(world, mission.Environment);
car.Body.OnCollision += new OnCollisionEventHandler(OnCollision);
gridDone = false;
pathDone = false;
pathSearchingDone = false;
bg = new Thread(() =>
{
DateTime now = DateTime.Now;
grid.BuildGVD();
console.WriteLine("GVD Generation Time: " + Math.Round((DateTime.Now - now).TotalMilliseconds) + " ms");
gridDone = true;
now = DateTime.Now;
pathSearching = true;
astar = HybridAStar.FindPath(grid, startPose, goalPose);
TimeSpan astarTime = DateTime.Now - now;
poses = astar.Path;
pathSearching = false;
pathSearchingDone = true;
if (astar.Path.Count > 0)
pathDone = true;
now = DateTime.Now;
if (smoothingOn)
smoothedPath = Smoother.Smooth(astar.Path, grid);
else
smoothedPath = astar.Path;
TimeSpan smoothingTime = DateTime.Now - now;
int numUnsafe = Smoother.UnsafeIndices != null ? Smoother.UnsafeIndices.Count : 0;
console.WriteLine("A*: Total Planning Time: " + Math.Round((astarTime + smoothingTime).TotalMilliseconds) + " ms");
console.WriteLine(" Heuristic Time: " + Math.Round(astar.HeuristicInitTime.TotalMilliseconds) + " ms");
console.WriteLine(" Searching Time: " + Math.Round((astarTime - astar.HeuristicInitTime).TotalMilliseconds) + " ms");
console.WriteLine(" Smoothing Time: " + Math.Round(smoothingTime.TotalMilliseconds) + " ms (" + Smoother.NumIterations + " iterations, " + Smoother.Change + "m, " + numUnsafe + " unsafe points)");
console.WriteLine(" " + astar.Discovered.Count + " nodes discovered");
console.WriteLine(" " + astar.Expanded.Count + " nodes expanded");
controller = new StanleyFSMController(smoothedPath, goalPose);
pathSmoothDone = true;
if (autoStart)
run = true;
});
bg.IsBackground = true;
bg.Priority = ThreadPriority.Lowest;
bg.Start();
}
