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);
}
