public IAction ReactTo(VehicleState state, int waypoint) { var projectedState = lastAction == null ? state : motionModel.CalculateNextState(state, lastAction, reactionTime).Last().state; projectedPosition.OnNext(projectedState.Position); var target = findClosestPoint(projectedState); selectedTarget.OnNext(path[target].State.Position); var throttle = 0.1; // target.Speed / vehicleModel.MaxSpeed; var steering = calculateSteering(state, target); var action = new SeekAction(throttle, steering); lastAction = action; return(action); }
private (VehicleState?, IAction?, bool) steer(TreeNode from, VehicleState to, DistanceMeasurement distances) { VehicleState?state = null; IAction? bestAction = null; bool reachedWayPoint = false; var shortestDistance = double.MaxValue; // todo what if there are no available actions? var availableActions = from.SelectAvailableActionsFrom(actions.AllPossibleActions).ToArray(); var remainingAvailableActions = availableActions.Length; foreach (var action in availableActions) { var predictedStates = motionModel.CalculateNextState(from.State, action, timeStep).ToList(); var elapsedTime = predictedStates.Last().relativeTime; var resultState = predictedStates.Last().state; var passedGoal = false; var collided = false; foreach (var(simulationTime, intermediateState) in predictedStates) { if (collisionDetector.IsCollision(intermediateState)) { elapsedTime = simulationTime; resultState = intermediateState; passedGoal = false; collided = true; break; } if (wayPoints[wayPointsReached].ReachedGoal(intermediateState.Position)) { passedGoal = true; } } if (collided) { from.DisableAction(action); remainingAvailableActions--; continue; } var currentDistance = distances.DistanceBetween(to, resultState); if ((!reachedWayPoint || passedGoal) && currentDistance < shortestDistance) { state = resultState; bestAction = action; reachedWayPoint = passedGoal; } } if (bestAction != null) { from.DisableAction(bestAction); remainingAvailableActions--; } if (bestAction == null || remainingAvailableActions == 0) { from.DisableFutureExpansions(); } return(state, bestAction, reachedWayPoint); }
public IPlan?FindOptimalPlanFor(VehicleState initialState) { var heuristic = createShortestPathHeuristic(initialState); var open = new BinaryHeapOpenSet <DiscreteState, SearchNode>(); var closed = new ClosedSet <DiscreteState>(); open.Add( new SearchNode( discreteState: discretizer.Discretize(initialState, wayPoints.Count), state: initialState, actionFromPreviousState: null, previousState: null, costToCome: 0, estimatedTotalCost: 0, // we don't need estimate for the initial node targetWayPoint: 0)); while (!open.IsEmpty) { var node = open.DequeueMostPromissing(); if (node.TargetWayPoint == wayPoints.Count) { return(node.ReconstructPlan()); } closed.Add(node.Key); exploredStates.OnNext(node.State); foreach (var action in actions.AllPossibleActions) { var predictedStates = motionModel.CalculateNextState(node.State, action, timeStep); var elapsedTime = predictedStates.Last().relativeTime; var nextVehicleState = predictedStates.Last().state; var reachedWayPoint = false; var collided = false; foreach (var(simulationTime, state) in predictedStates) { if (collisionDetector.IsCollision(state)) { elapsedTime = simulationTime; nextVehicleState = state; reachedWayPoint = false; collided = true; break; } if (wayPoints[node.TargetWayPoint].ReachedGoal(state.Position)) { reachedWayPoint = true; } } var nextTargetWayPoint = reachedWayPoint ? node.TargetWayPoint + 1 : node.TargetWayPoint; var nextDiscreteState = discretizer.Discretize(nextVehicleState, nextTargetWayPoint); if (closed.Contains(nextDiscreteState)) { continue; } if (collided) { closed.Add(nextDiscreteState); continue; } var costToCome = node.CostToCome + timeStep.TotalSeconds; var reachedLastWayPoint = nextTargetWayPoint != wayPoints.Count; var costToGo = reachedLastWayPoint ? heuristic.EstimateTimeToGoal(nextVehicleState, nextTargetWayPoint).TotalSeconds : 0; var discoveredNode = new SearchNode( discreteState: nextDiscreteState, state: nextVehicleState, actionFromPreviousState: action, previousState: node, costToCome: costToCome, estimatedTotalCost: costToCome + costToGo, nextTargetWayPoint); if (greedy && reachedLastWayPoint) { return(discoveredNode.ReconstructPlan()); } if (!open.Contains(discoveredNode.Key)) { open.Add(discoveredNode); } else if (discoveredNode.CostToCome < open.Get(discoveredNode.Key).CostToCome) { open.ReplaceExistingWithTheSameKey(discoveredNode); } } } return(null); }