// 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;
}
// 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 ();
}
// 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;
}
private void _addNewEmptyGlobalPathWayPointAtIndex(int i)
{
// we are statically allocating one grid waypoint as we are creating one corresponding grid navigation task which will watch it
globalPath.Add(new State());
globalPathPolygonIndices.Add(0); // adding dummy polygon index
offMeshLinkWayPoint.Add(false); // adding off mesh link state defaults to false
// creating the grid task
List <State> gridPath = new List <State> ();
localPaths.Add(gridPath);
State gridStartState = (i == 0) ? currentState : globalPath[i - 1];
// tunnel planner stuff
GridTimeDomain gridTimeDomain = new GridTimeDomain(neighborObstacles, neighborAgents);
List <State> spaceTimePath = new List <State> ();
spaceTimePaths.Add(spaceTimePath);
GridNavigationTask gridNavigationTask = new GridNavigationTask(gridStartState, globalPath[i], 2.5F, ref gridPath,
1.0f, 1.0f, ref spaceTimePath, gridTimeDomain,
TaskPriority.Inactive, taskManager);
gridNavigationTask.taskName = "gridNavigationTask:" + i.ToString();
globalPath[i].notifyObservers(Event.GOAL_INVALID); // currently the goal is invalid
gridNavigationTasks.Add(gridNavigationTask);
if (i == 0) // first grid navigation task
{
neighborAgents.observable.registerObserver(Event.NEIGBHOR_AGENTS_CHANGED, gridNavigationTask);
neighborObstacles.observable.registerObserver(Event.NEIGBHOR_DYNAMIC_OBSTACLES_CHANGED, gridNavigationTask);
gridNavigationTask.currentlyExecutingTask = true;
}
/*
* // creating the grid time task
* List<State> spaceTimePath = new List<State> ();
* spaceTimePaths.Add(spaceTimePath);
*
* GridTimeDomain gridTimeDomain = new GridTimeDomain (neigbhorObstacles);
* GridTimeTunnelNavigationTask tunnelNavigationTask =
* new GridTimeTunnelNavigationTask(gridStartState, globalPath[i], gridPath,1.0F,1.0F,ref gridTimeDomain, ref spaceTimePath, TaskPriority.Inactive,taskManager);
* tunnelNavigationTask.taskName = "tunnelNavigationTask" + i.ToString();
*
* // EVENT: gridNavigationTask will send Event.GRID_PATH_CHANGED to tunnelNavigationTask
* gridNavigationTask.registerObserver(Event.GRID_PATH_CHANGED,tunnelNavigationTask);
*/
}
// Use this for initialization
public override void Init(AnimationAnalyzer animAnalyzer, NeighbourAgents neighborhood, NeighbourObstacles obstacles)
{
analyzer = animAnalyzer;
if (analyzer != null && !analyzer.initialized)
{
analyzer.Init();
}
storedPlan = new List <FootstepPlanningAction>();
outputPlan = new Stack <DefaultAction>();
domainList = new List <PlanningDomainBase>();
gpd = new GridPlanningDomain(analyzer);
//gtd = new GridTimeDomain(analyzer, obstacles);
gtd = new GridTimeDomain(obstacles, neighborhood);
fpd = new FootstepPlanningDomain(analyzer, neighborhood, obstacles);
if (useGridDomain)
{
domainList.Add(gpd);
//domainList.Add(gpd);
}
if (useGridTimeDomain)
{
domainList.Add(gtd);
//domainList.Add(gtd);
}
if (useFootstepDomain)
{
domainList.Add(fpd);
//domainList.Add(fpd);
}
planner = new BestFirstSearchPlanner();
planner.init(ref domainList, MaxNumberOfNodes);
initialized = true;
currentState = null;
isPlanComputed = false;
planChanged = false;
goalMeshRenderer = goalStateTransform.gameObject.GetComponent("MeshRenderer") as MeshRenderer;
currentGoal = goalStateTransform.position;
goalReached = false;
}
// defaults to a real-time task unless the priority is explicitly set
public GridTimeTunnelNavigationTask_DEPRECATED(State t_startState, State t_goalState, List<State> t_tunnel, float t_tunnelDistanceThreshold, float t_tunnelTimeThreshold,
ref GridTimeDomain t_gridTimeDomain, ref List<State> t_spaceTimePath,
TaskPriority t_taskPriority, TaskManager taskManager)
: base(taskManager, t_taskPriority)
{
startState = t_startState;
goalState = t_goalState;
taskType = TaskType.GridTimeTunnelNavigationTask_DEPRECATED;
// this IS a reference -- we never actually assign it again -- it just refers the path of the grid task
tunnel = t_tunnel;
tunnelDistanceThreshold = t_tunnelDistanceThreshold;
tunnelTimeThreshold = t_tunnelTimeThreshold;
gridTimeDomain = t_gridTimeDomain;
spaceTimePath = t_spaceTimePath;
}
public GridTimeTunnelNavigationTask_DEPRECATED(State t_startState, State t_goalState, List <State> t_tunnel, float t_tunnelDistanceThreshold, float t_tunnelTimeThreshold,
ref GridTimeDomain t_gridTimeDomain, ref List <State> t_spaceTimePath,
TaskPriority t_taskPriority, TaskManager taskManager)
: base(taskManager, t_taskPriority) // defaults to a real-time task unless the priority is explicitly set
{
startState = t_startState;
goalState = t_goalState;
taskType = TaskType.GridTimeTunnelNavigationTask_DEPRECATED;
// this IS a reference -- we never actually assign it again -- it just refers the path of the grid task
tunnel = t_tunnel;
tunnelDistanceThreshold = t_tunnelDistanceThreshold;
tunnelTimeThreshold = t_tunnelTimeThreshold;
gridTimeDomain = t_gridTimeDomain;
spaceTimePath = t_spaceTimePath;
}
// 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);
}
}
private void _addNewEmptyGlobalPathWayPointAtIndex(int i)
{
// we are statically allocating one grid waypoint as we are creating one corresponding grid navigation task which will watch it
globalPath.Add(new State());
globalPathPolygonIndices.Add(0); // adding dummy polygon index
offMeshLinkWayPoint.Add(false); // adding off mesh link state defaults to false
// creating the grid task
List<State> gridPath = new List<State> ();
localPaths.Add(gridPath);
State gridStartState = (i == 0) ? currentState : globalPath[i-1];
// tunnel planner stuff
GridTimeDomain gridTimeDomain = new GridTimeDomain (neighborObstacles, neighborAgents);
List<State> spaceTimePath = new List<State> ();
spaceTimePaths.Add(spaceTimePath);
GridNavigationTask gridNavigationTask = new GridNavigationTask (gridStartState, globalPath[i],2.5F, ref gridPath,
1.0f, 1.0f, ref spaceTimePath, gridTimeDomain,
TaskPriority.Inactive, taskManager);
gridNavigationTask.taskName = "gridNavigationTask:" + i.ToString();
globalPath[i].notifyObservers(Event.GOAL_INVALID); // currently the goal is invalid
gridNavigationTasks.Add(gridNavigationTask);
if (i==0) // first grid navigation task
{
neighborAgents.observable.registerObserver(Event.NEIGBHOR_AGENTS_CHANGED, gridNavigationTask);
neighborObstacles.observable.registerObserver(Event.NEIGBHOR_DYNAMIC_OBSTACLES_CHANGED, gridNavigationTask);
gridNavigationTask.currentlyExecutingTask = true;
}
/*
// creating the grid time task
List<State> spaceTimePath = new List<State> ();
spaceTimePaths.Add(spaceTimePath);
GridTimeDomain gridTimeDomain = new GridTimeDomain (neigbhorObstacles);
GridTimeTunnelNavigationTask tunnelNavigationTask =
new GridTimeTunnelNavigationTask(gridStartState, globalPath[i], gridPath,1.0F,1.0F,ref gridTimeDomain, ref spaceTimePath, TaskPriority.Inactive,taskManager);
tunnelNavigationTask.taskName = "tunnelNavigationTask" + i.ToString();
// EVENT: gridNavigationTask will send Event.GRID_PATH_CHANGED to tunnelNavigationTask
gridNavigationTask.registerObserver(Event.GRID_PATH_CHANGED,tunnelNavigationTask);
*/
}
// 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 ();
}
// Use this for initialization
public override void Init(AnimationAnalyzer animAnalyzer, NeighbourAgents neighborhood, NeighbourObstacles obstacles)
{
analyzer = animAnalyzer;
if (analyzer != null && !analyzer.initialized)
analyzer.Init();
storedPlan = new List<FootstepPlanningAction>();
outputPlan = new Stack<DefaultAction>();
domainList = new List<PlanningDomainBase>();
gpd = new GridPlanningDomain(analyzer);
//gtd = new GridTimeDomain(analyzer, obstacles);
gtd = new GridTimeDomain(obstacles,neighborhood);
fpd = new FootstepPlanningDomain(analyzer, neighborhood, obstacles);
if (useGridDomain)
{
domainList.Add(gpd);
//domainList.Add(gpd);
}
if (useGridTimeDomain)
{
domainList.Add(gtd);
//domainList.Add(gtd);
}
if (useFootstepDomain)
{
domainList.Add(fpd);
//domainList.Add(fpd);
}
planner = new BestFirstSearchPlanner();
planner.init(ref domainList, MaxNumberOfNodes);
initialized = true;
currentState = null;
isPlanComputed = false;
planChanged = false;
goalMeshRenderer = goalStateTransform.gameObject.GetComponent("MeshRenderer") as MeshRenderer;
currentGoal = goalStateTransform.position;
goalReached = false;
}
