% go to rvctools/ and run startup_rvc
% first example is for you to get familiar with the planning 
% module of the toolbox. 

% loads a map
%load map1

% check what gets loaded
%who

% instantiate an bug class navigation methods
%bug = Bug2(map)
 
% set the goal 
%bug.goal = [50, 35]; 

% find the path to the goal from the initial condition 
% it will display the simulation

%bug.path([20,10]);

% you can also run EDU>> p = bug.path([20,10]);

% here are some commands how you can make your own map

map = zeros(100, 100);

% you can add obstacles by simply saying 

map(40:50, 20:70) = 1;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%low cost terrain
map(60:70, 20:70) = .0000001;
%high cost terrain
map(40:50, 20:70) = 3;

ds = Dstar(map);
c = ds.costmap();
goal = [50, 80]; 
ds.plan(goal);
p = ds.path([40,10]);
ds.visualize(p)

% 1.
% For the D* planner (dynamic version of A* planner, which enables
% you to efficiently change the cost dymamically )increase the cost
% of the rough terrain 
% and observe what happens
% Add a region of very low-cost terrain (less then one) 
% near the robots path and observe what happens. 

% 2. This problem lets you try and modify Rapidly Exploring Random 
% tree planner.  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% RRT planner 
% RRT roadmap for obstacle free environment is created and
% visualized as follows
%rrt = RRT(); 
%rrt.plan(); 
%rrt.visualize(); 

% note that the paths have good coverage in the configuration 
% space (x,y,theta) - note that the plot is 3D dimensional

% to plan a path, you simply call rrt with initial and starting position
%p = rrt.path([0 0 0],[2 3 0]); 

% 1. Create a new RRT by making the integration limit and steering
% angle limits

% 2. Current RRT chooses a streering angle from uniform distribution
% between streering angle limits. As people tend to drive more smoothly
% observe what happens of the streering angle is chosen from normal
% Gaussian ditribution. 

% 3. Add some obstacles and plot the path before and after
% obstacles were added. 



