public IPath GetPathToGoal()
{
if (waypoints.Length == 0.0)
{
return(null);
}
lock (locker)
{
RobotTwoWheelState currentState = stateProvider.GetCurrentState(lastCommand);
IPath path = new PointPath();
RRTNode startingNode = new RRTNode(currentState);
RRTNode goalNode;
//foreach (IPathSegment segment in waypoints)
//{
while (rrt.FindPath(ref startingNode, waypoints[1].End, obstacles, out goalNode) == false)
{
continue;
}
//startingNode = goalNode;
IPath segmentToGoal = GetFinalPath(goalNode);
// add each small segment of each segment to the path to return
//foreach (IPathSegment seg in segmentToGoal)
//path.Add(seg);
//}
return(segmentToGoal);
}
}
public RRTNode GetNodeToGoal()
{
if (waypoints.Length == 0.0)
{
return(null);
}
lock (locker)
{
RobotTwoWheelState currentState = stateProvider.GetCurrentState(lastCommand);
IPath path = new PointPath();
RRTNode startingNode = new RRTNode(currentState);
RRTNode goalNode;
//foreach (IPathSegment segment in waypoints)
//{
bool foundPath = false;
Console.WriteLine("Finding a path");
do
{
startingNode = new RRTNode(currentState);
foundPath = rrt.FindPath(ref startingNode, waypoints[1].End, obstacles, out goalNode);
}while (!foundPath && receivedNewPath);
receivedNewPath = false;
return(goalNode);
}
}
public RobotTwoWheelState GetCurrentState(RobotTwoWheelCommand currentCommand)
{
RobotWheelModel rightW = new RobotWheelModel(rWheelPos, rWheelVel, rWheelAccel);
RobotWheelModel leftW = new RobotWheelModel(lWheelPos, lWheelVel, lWheelAccel);
RobotTwoWheelState state = new RobotTwoWheelState(curPose, currentCommand, rightW, leftW);
return(new RobotTwoWheelState(state, currentCommand));
}
public MonteCarloFollower(double lookAhead, double maxVelocity, double maxTurn)
{
DISTANCE_WEIGHT = 25;
VELOCITY_WEIGHT = 1;
TURN_WEIGHT = 1;
this.lookAhead = lookAhead;
this.maxVelocity = maxVelocity;
this.maxTurn = maxTurn;
state = new RobotTwoWheelState(new RobotPose(), new RobotTwoWheelCommand(0, 0), new RobotWheelModel(0, 0, 0), new RobotWheelModel(0, 0, 0));
}
private double GetCost(RobotTwoWheelCommand command)
{
double lookAheadRemaining = lookAhead;
double cost = 0;
PointOnPath closestPoint = path.GetClosest(state.Pose.ToVector2());
PointOnPath lookAheadPoint = path.AdvancePoint(closestPoint, ref lookAheadRemaining);
RobotTwoWheelState newState = RobotTwoWheelModel.Simulate(command, state, (lookAhead - lookAheadRemaining) / command.velocity);
return(newState.Pose.ToVector2().DistanceTo(lookAheadPoint.pt) * DISTANCE_WEIGHT); // - command.velocity * VELOCITY_WEIGHT - command.turn * TURN_WEIGHT;
}
/// <summary>
/// Calculate a point starting from a RRTNode, while avoiding obstacles
/// </summary>
/// <param name="startNode">The node that path predicted from</param>
/// <param name="vInput">Velocity control input</param>
/// <param name="wInput">Turn Rate control input</param>
/// <param name="obstacles">Obstalces (List of Polygon)</param>
/// <returns>a RRTNode that has startNode as its parent. Returns null if fails</returns>
private RRTNode CalculateNextNode(RRTNode startNode, double vInput, double wInput, List <Polygon> obstacles)
{
//4) Divide the total RRT timestep into smaller sub-steps
//4a) calculate the trajectory at one substep given the control inputs and closest node initial conditions
//4b) check at each the linear path between the initial and simulation end does not intersect a polygon
//4c) if intersects, terminate and go to 1.
//4d) if not intersects
//4da) if last substep, add the results of this simulation to the closest node as a child
//4db) else simulate the next subtime step by going to 4a
RobotTwoWheelState lastSliceState = startNode.State;
RobotTwoWheelState curSliceState = startNode.State;
List <Vector2> simulationPoints = new List <Vector2>();
for (int i = 0; i < numSlice; i++)
{
simulationTimer.Start();
curSliceState = RobotTwoWheelModel.Simulate(new RobotTwoWheelCommand(vInput, wInput), lastSliceState, (timeStep / numSlice));
simulationTime.Add(simulationTimer.ElapsedMilliseconds);
simulationTimer.Reset();
obstacleTimer.Start();
if (IsHittingObstacle(lastSliceState.Pose.ToVector2(), curSliceState.Pose.ToVector2(), obstacles))
{
return(null);
}
obstacleTime.Add(obstacleTimer.ElapsedMilliseconds);
obstacleTimer.Reset();
simulationPoints.Add(lastSliceState.Pose.ToVector2());
lastSliceState = curSliceState;
}
simulationPoints.Add(lastSliceState.Pose.ToVector2());
RRTNode r = new RRTNode(curSliceState, startNode);
r.simulationPoints = simulationPoints;
return(r);
//return new RRTNode(rOut, vOut, wOut, 0, startNode);
}
public RRTNode(RobotTwoWheelState state)
{
this.state = state;
this.parent = this;
}
public RRTNode(RobotTwoWheelState state, RRTNode parent)
{
this.state = state;
this.parent = parent;
}