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();
}
public static Mission LoadFromJson(string fileName, World world)
{
JObject o = (JObject)JToken.ReadFrom(new JsonTextReader(File.OpenText(fileName)));
Mission m;
if (o["mission"] != null)
{
m = MissionFactory.CreateMission((MissionFactory.MissionType)Enum.Parse(typeof(MissionFactory.MissionType), (string)o["mission"]), world);
}
else
{
m = new Mission();
m.Start = parsePose(o["start"]);
m.Goal = parsePose(o["goal"]);
m.Environment = parseEnvironment(o["environment"], world, m.Start.Position, m.Goal.Position);
if (o["epsilon"] != null)
m.AStarEpsilon = (float)o["epsilon"];
if (o["resolution"] != null)
m.AStarGridResolution = (float)o["resolution"];
}
return m;
}
private void startSim()
{
simulation.ClearWorld();
start = DateTime.Now;
Pose startPose, goalPose;
Mission m = new Mission();
m.Environment = buildEnvironment(startIndex, goalIndex, out startPose, out goalPose);
m.Start = startPose;
m.Goal = goalPose;
simulation.InitSim(true, true, m);
}