Exemple #1
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);
        }