/// <summary> /// Performs the necessary updates after start moved. /// </summary> /// <param name='currentState'> /// Current state. /// </param> public void UpdateAfterStartMoved(DefaultState currentState) { if (Plan.ContainsState(currentState)) { float actionCost = Plan.Node(currentState).action.cost; foreach (ARAstarNode node in Plan.Elements().Values) { node.g -= actionCost; } Visited.UpdateList(Plan.Node(currentState)); } else { // TODO : what if the current state is not part of the plan ? // can we treat this like the obstacle movement splitting up the search graph firstTime = true; inflationFactor = 2.5f; Open.Clear(); Close.Clear(); Plan.Clear(); Incons.Clear(); createStartNode(ref currentState, ref goalState); Open.Insert(startNode); Visited.insertNode(ref startNode); Open.startState = startNode.action.state; } moved = false; }
//An edge is just a node using previous state, action cost as the edge cost, and action state public void InifiniteUpdate(ref Dictionary <DefaultState[], Edge> edges, ref Dictionary <DefaultState, ADAstarNode> nodes, ref Stack <DefaultAction> plan, float maxTime, ref DefaultState startState) { float score = 0.0f; PlanningDomainBase domain = default(PlanningDomainBase); foreach (PlanningDomainBase d in _planningDomain) { if (d.evaluateDomain(ref startState) > score) { score = d.evaluateDomain(ref startState); domain = d; } } /*float maxChange = 0.0f; * if(changeInEdgeCost || inflationFactor != 1.0f) * { * * foreach(Edge edge in edges.Values) * { * if(edge.previousCost != edge.cost) * { * maxChange = Mathf.Abs(edge.cost - edge.previousCost); * edge.previousCost = edge.cost; * ADAstarNode n = nodes[edge.u]; * UpdateState(ref n); * } * } * } * * if(maxChange > edgeChangeThreshold) //Decide either increase inflation factor or replan from scratch * { * if(PLANNER_MODE.MODE.Equals(PlannerMode.PLANNING_MODE.IncreaseFactor)){ * inflationFactor += .1f; * } * else if(PLANNER_MODE.MODE.Equals(PlannerMode.PLANNING_MODE.FromScratch)){ * ComputeorImprovePath(ref domain, maxTime ); * } * // inflationFactor += .5; // Search for a good value to increase * //OR * //ComputeorImprovePath(); * } */ //else if (inflationFactor > 1.0f) { inflationFactor -= .2f; //Decide decrease amount } //Decrease inflationFactor by some amount //Move states from incons to open foreach (KeyValuePair <DefaultState, ADAstarNode> keyVal in Incons) { Open.Add(keyVal); } Incons.Clear(); foreach (KeyValuePair <DefaultState, ADAstarNode> keyval in Open) { keyval.Value.key1 = GetKey(keyval.Value)[0]; keyval.Value.key2 = GetKey(keyval.Value)[1]; } Open.Sort(ADAstartCopareCost.CompareCost); //Closed.Clear(); //computeimprove path ComputeorImprovePath(ref domain, maxTime); //publish solution }