Esempio n. 1
0
        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();
        }
Esempio n. 2
0
        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();
        }
Esempio n. 3
0
        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();
        }