// Use this for initialization
void Start()
{
planner = new ARAstarPlanner();
BestFirstPlanner = new BestFirstSearchPlanner();
GetComponent<NeighbourObstacles>().Init ();
GridTimeDomain domain = new GridTimeDomain(GetComponent<NeighbourObstacles>(), GetComponent<NeighbourAgents>());
List<PlanningDomainBase> dl = new List<PlanningDomainBase>();
dl.Add(domain);
DefaultState startState = new GridTimeState(transform.position) as DefaultState;
DefaultState goalState = new GridTimeState(new Vector3(5,0.5f,0),5.0f) as DefaultState;
plan = new Dictionary<DefaultState, ARAstarNode>();
outputPlan = new Stack<DefaultState>();
if(BestFirstSearchPlanner) {
BestFirstPlanner.init(ref dl, 200);
bool completed = BestFirstPlanner.computePlan(ref startState, ref goalState, ref outputPlan, 10.0f);
Debug.Log("BestFirst Plan " + outputPlan.Count);
Debug.Log("Plan found: " + completed);
} else {
planner.init(ref dl,200);
float inflationFactor = 2.5f;
PathStatus status = planner.computePlan(ref startState ,ref goalState ,ref plan,ref inflationFactor, 10.0f);
planner.FillPlan();
Debug.Log ("ARA Plan " + plan.Count);
Debug.Log("Nodes Expanded: " + planner.Close.Elements().Count);
Debug.Log("Status: " + status);
}
}
public GridNavigationTask(State t_startState, State t_goalState, float t_inflationFactor, ref List <State> t_path,
float t_tunnelDistanceThreshold, float t_tunnelTimeThreshold, ref List <State> t_spaceTimePath, GridTimeDomain t_gridTimeDomain,
TaskPriority t_taskPriority, TaskManager taskManager)
: base(taskManager, t_taskPriority) // defaults to a real-time task -- now what happens when both this and Global are first put in queue ??
{
taskType = TaskType.GridNavigationTask;
startState = t_startState;
goalState = t_goalState;
// EVENT : startState will trigger GridNavigationTask that STATE_CHANGED
startState.registerObserver(Event.STATE_POSITION_CHANGED, this);
// EVENT : goalState will trigger GridNavigationTask that STATE_CHANGED
goalState.registerObserver(Event.STATE_POSITION_CHANGED, this);
// EVENT : goalState will trigger GridNavigationTask that it is GOAL_INVALID or VALID
goalState.registerObserver(Event.GOAL_INVALID, this);
goalState.registerObserver(Event.GOAL_VALID, this);
inflationFactor = t_inflationFactor;
//planner = t_planner;
outputPlan = new Dictionary <DefaultState, ARAstarNode>();
path = t_path;
// TODO : deprecate these lists
List <PlanningDomainBase> domainList = new List <PlanningDomainBase>();
ARAstarDomain araStarDomain = new ARAstarDomain();
araStarDomain.setPlanningTask(this);
domainList.Add(araStarDomain);
gridPlanner = new ARAstarPlanner();
gridPlanner.init(ref domainList, 100);
initialized = true;
obstacleChanged = false;
obstacleChangedData = new List <object> ();
// tunnel planner variables
tunnelDistanceThreshold = t_tunnelDistanceThreshold;
tunnelTimeThreshold = t_tunnelTimeThreshold;
gridTimeDomain = t_gridTimeDomain;
spaceTimePath = t_spaceTimePath;
spaceTimePathStatus = false;
dynamicChange = false;
currentlyExecutingTask = false;
gridPathStatus = PathStatus.NoPath;
}
public OpenContainer(ARAstarPlanner planner, bool _useHeap = true)
{
parentPlanner = planner;
openList = new List <ARAstarNode> ();
openHeap = new ARAstarHeap(planner.inflationFactor);
openDictionary = new Dictionary <DefaultState, ARAstarNode> ();
useHeap = _useHeap;
Debug.Log("Using Heap: " + useHeap);
}
// defaults to a real-time task -- now what happens when both this and Global are first put in queue ??
public GridNavigationTask(State t_startState, State t_goalState, float t_inflationFactor, ref List<State> t_path,
float t_tunnelDistanceThreshold, float t_tunnelTimeThreshold, ref List<State> t_spaceTimePath, GridTimeDomain t_gridTimeDomain,
TaskPriority t_taskPriority, TaskManager taskManager)
: base(taskManager, t_taskPriority)
{
taskType = TaskType.GridNavigationTask;
startState = t_startState;
goalState = t_goalState;
// EVENT : startState will trigger GridNavigationTask that STATE_CHANGED
startState.registerObserver(Event.STATE_POSITION_CHANGED, this);
// EVENT : goalState will trigger GridNavigationTask that STATE_CHANGED
goalState.registerObserver(Event.STATE_POSITION_CHANGED, this);
// EVENT : goalState will trigger GridNavigationTask that it is GOAL_INVALID or VALID
goalState.registerObserver(Event.GOAL_INVALID, this);
goalState.registerObserver(Event.GOAL_VALID, this);
inflationFactor = t_inflationFactor;
//planner = t_planner;
outputPlan = new Dictionary<DefaultState, ARAstarNode>();
path = t_path;
// TODO : deprecate these lists
List<PlanningDomainBase> domainList = new List<PlanningDomainBase>();
ARAstarDomain araStarDomain = new ARAstarDomain ();
araStarDomain.setPlanningTask(this);
domainList.Add(araStarDomain);
gridPlanner = new ARAstarPlanner();
gridPlanner.init(ref domainList, 100);
initialized = true;
obstacleChanged = false;
obstacleChangedData = new List<object> ();
// tunnel planner variables
tunnelDistanceThreshold = t_tunnelDistanceThreshold;
tunnelTimeThreshold = t_tunnelTimeThreshold;
gridTimeDomain = t_gridTimeDomain;
spaceTimePath = t_spaceTimePath;
spaceTimePathStatus = false;
dynamicChange = false;
currentlyExecutingTask = false;
gridPathStatus = PathStatus.NoPath;
}
// Use this for initialization
void Start()
{
actions = new Stack <ARAstarAction>();
outputPlan = new Dictionary <DefaultState, ARAstarNode>();
domainList = new List <PlanningDomainBase>();
domainList.Add(new ARAstarDomain());
planner = new ARAstarPlanner();
planner.init(ref domainList, 100);
planner.usingHeap = usingHeap;
startState = new ARAstarState(startObject.transform.position);
goalState = new ARAstarState(goalObject.transform.position);
prevGoalState = goalState;
DStartState = startState as DefaultState;
DGoalState = goalState as DefaultState;
neighborMap = new GridNeighbors();
}
// Use this for initialization
void Start()
{
planner = new ARAstarPlanner();
BestFirstPlanner = new BestFirstSearchPlanner();
GetComponent <NeighbourObstacles>().Init();
GridTimeDomain domain = new GridTimeDomain(GetComponent <NeighbourObstacles>(), GetComponent <NeighbourAgents>());
List <PlanningDomainBase> dl = new List <PlanningDomainBase>();
dl.Add(domain);
DefaultState startState = new GridTimeState(transform.position) as DefaultState;
DefaultState goalState = new GridTimeState(new Vector3(5, 0.5f, 0), 5.0f) as DefaultState;
plan = new Dictionary <DefaultState, ARAstarNode>();
outputPlan = new Stack <DefaultState>();
if (BestFirstSearchPlanner)
{
BestFirstPlanner.init(ref dl, 200);
bool completed = BestFirstPlanner.computePlan(ref startState, ref goalState, ref outputPlan, 10.0f);
Debug.Log("BestFirst Plan " + outputPlan.Count);
Debug.Log("Plan found: " + completed);
}
else
{
planner.init(ref dl, 200);
float inflationFactor = 2.5f;
PathStatus status = planner.computePlan(ref startState, ref goalState, ref plan, ref inflationFactor, 10.0f);
planner.FillPlan();
Debug.Log("ARA Plan " + plan.Count);
Debug.Log("Nodes Expanded: " + planner.Close.Elements().Count);
Debug.Log("Status: " + status);
}
}
public OpenContainer(ARAstarPlanner planner, bool _useHeap = true)
{
parentPlanner = planner;
openList = new List<ARAstarNode> ();
openHeap = new ARAstarHeap(planner.inflationFactor);
openDictionary = new Dictionary<DefaultState, ARAstarNode> ();
useHeap = _useHeap;
Debug.Log("Using Heap: " + useHeap);
}
// Creation function
//public GridTunnelSearch(Stack<DefaultAction> plan, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_x, float T_z, float T_t)
public GridTunnelSearch(Stack <DefaultAction> plan, Vector3 startPos, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_d, float T_t, float startSpeed = 0)
{
currentPlan = new DefaultAction[plan.Count];
plan.CopyTo(currentPlan, 0);
this.T_d = T_d;
//this.T_x = T_x;
//this.T_z = T_z;
this.T_t = T_t;
this.maxTime = MaxTime;
int startIndex = 0;
//GridPlanningState startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//while (startPlanningState == null)
//{
// startIndex++;
// startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//}
//startState = new GridTimeState(startPlanningState.currentPosition, startTime, startSpeed);
startState = new GridTimeState(startPos, startTime, startSpeed);
lastState = new GridTimeState((currentPlan[currentPlan.Length - 1].state as GridPlanningState).currentPosition, idealGoalTime);
gridTimeDomain = gtd;
float dist = (startState.currentPosition - lastState.currentPosition).magnitude;
// TODO: what to do if there is no time???
float minSpeed = dist / (idealGoalTime - startTime);
if (minSpeed < 0)
{
minSpeed = GridTimeDomain.MIN_SPEED;
}
//tunnel = ConvertPlan(gtd.minSpeed, gtd.midSpeed, gtd.maxSpeed);
tunnel = ConvertPlan(startTime, minSpeed, GridTimeDomain.MID_SPEED, GridTimeDomain.MAX_SPEED);
//for(int aux = 0; aux < tunnel.tunnelMinVelocity.Length; aux++)
// Debug.Log("Tunnel "+aux+":"+tunnel.tunnelMinVelocity[aux]);
tunnelPlanner = new BestFirstSearchPlanner();
List <PlanningDomainBase> domainList = new List <PlanningDomainBase>();
domainList.Add(gridTimeDomain);
tunnelPlanner.init(ref domainList, MaxNumberOfNodes);
// MUBBASIR TESTING ARA STAR PLANNER HERE
araStarPlanner = new ARAstarPlanner();
araStarPlanner.init(ref domainList, MaxNumberOfNodes);
goalReached = false;
ComputeTunnelSearch();
//ComputeTunnelSearchStatePlan ();
}
// Creation function
//public GridTunnelSearch(Stack<DefaultAction> plan, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_x, float T_z, float T_t)
public GridTunnelSearch(Stack<DefaultAction> plan, Vector3 startPos, float startTime, float idealGoalTime, GridTimeDomain gtd, int MaxNumberOfNodes, float MaxTime, float T_d, float T_t, float startSpeed = 0)
{
currentPlan = new DefaultAction[plan.Count];
plan.CopyTo(currentPlan,0);
this.T_d = T_d;
//this.T_x = T_x;
//this.T_z = T_z;
this.T_t = T_t;
this.maxTime = MaxTime;
int startIndex = 0;
//GridPlanningState startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//while (startPlanningState == null)
//{
// startIndex++;
// startPlanningState = currentPlan[startIndex].state as GridPlanningState;
//}
//startState = new GridTimeState(startPlanningState.currentPosition, startTime, startSpeed);
startState = new GridTimeState(startPos, startTime, startSpeed);
lastState = new GridTimeState((currentPlan[currentPlan.Length-1].state as GridPlanningState).currentPosition, idealGoalTime);
gridTimeDomain = gtd;
float dist = (startState.currentPosition - lastState.currentPosition).magnitude;
// TODO: what to do if there is no time???
float minSpeed = dist / (idealGoalTime-startTime);
if (minSpeed < 0)
minSpeed = GridTimeDomain.MIN_SPEED;
//tunnel = ConvertPlan(gtd.minSpeed, gtd.midSpeed, gtd.maxSpeed);
tunnel = ConvertPlan(startTime, minSpeed, GridTimeDomain.MID_SPEED, GridTimeDomain.MAX_SPEED);
//for(int aux = 0; aux < tunnel.tunnelMinVelocity.Length; aux++)
// Debug.Log("Tunnel "+aux+":"+tunnel.tunnelMinVelocity[aux]);
tunnelPlanner = new BestFirstSearchPlanner();
List<PlanningDomainBase> domainList = new List<PlanningDomainBase>();
domainList.Add(gridTimeDomain);
tunnelPlanner.init(ref domainList, MaxNumberOfNodes);
// MUBBASIR TESTING ARA STAR PLANNER HERE
araStarPlanner = new ARAstarPlanner ();
araStarPlanner.init(ref domainList, MaxNumberOfNodes);
goalReached = false;
ComputeTunnelSearch();
//ComputeTunnelSearchStatePlan ();
}
public NodeComparer(ARAstarPlanner planner)
{
parentPlanner = planner;
}
