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