void rrt_t::step()
{
random_sample();
nearest_vertex();
if(propagate())
{
add_to_tree();
}
}
void adaptive_sparse_rrt_t::step()
{
//sample a state
sampler->sample(state_space,sample_point);
//get the nearest state
tree_vertex_index_t nearest = nearest_vertex(sample_point);
//propagate toward the sampled point
plan_t plan;
plan.link_control_space(control_space);
state_t* end_state = pre_alloced_points[point_number];
double prop_length=0;
int attempts=0;
trajectory.clear();
do
{
if(collision_checking)
{
plan.clear();
local_planner->steer(tree[nearest]->point,sample_point,plan,trajectory,false);
state_space->copy_point(end_state,trajectory[trajectory.size()-1]);
prop_length = plan.length();
}
else
{
plan.clear();
local_planner->steer(tree[nearest]->point,sample_point,plan,end_state,false);
prop_length = plan.length();
}
attempts++;
}
while(drain && attempts < max_attempts && ( metric->distance_function(tree[nearest]->point,end_state) < delta_drain || metric->distance_function(tree[nearest]->point,end_state) == PRX_INFINITY));
count_of_failure += attempts-1;
//check if the trajectory is valid
if(!collision_checking || (validity_checker->is_valid(trajectory) && trajectory.size()>1))
{
std::vector<tree_vertex_index_t> X_near;
double new_cost = get_vertex(nearest)->cost + prop_length;
bool better_node = false;
//Check if a node inside the radius_nearest region
if(drain)
{
X_near = neighbors(end_state);
foreach(tree_vertex_index_t x_near, X_near)
{
if(get_vertex(x_near)->cost <= new_cost )
{
better_node = true;
}
}
}
if(!better_node)
{
tree_vertex_index_t v = tree.add_vertex<adaptive_sparse_rrt_node_t,adaptive_sparse_rrt_edge_t>();
adaptive_sparse_rrt_node_t* node = get_vertex(v);
node->point = end_state;
// PRX_INFO_S("POINT: "<< state_space->print_point(end_state));
point_number++;
node->bridge = true;
node->cost = get_vertex(nearest)->cost + prop_length;
tree_edge_index_t e = tree.add_edge<adaptive_sparse_rrt_edge_t>(nearest,v);
get_edge(e)->plan = plan;
state_space->copy_point(states_to_check[0],end_state);
//check if better goal node
if(!real_solution && input_query->get_goal()->satisfied(end_state))
{
best_goal = v;
real_solution = true;
}
else if(real_solution && get_vertex(best_goal)->cost > get_vertex(v)->cost && input_query->get_goal()->satisfied(end_state) )
{
best_goal = v;
}
//check if we need to store trajectories for visualization
if(collision_checking && visualize_tree)
{
get_edge(e)->trajectory = trajectory;
}
else if(!collision_checking && visualize_tree)
{
if(trajectory.size()==0)
local_planner->propagate(get_vertex(nearest)->point,plan,trajectory);
get_edge(e)->trajectory = trajectory;
}
if(drain)
{
foreach(tree_vertex_index_t x_near, X_near)
{
if(!get_vertex(x_near)->bridge )
{
metric->remove_point(tree[x_near]);
get_vertex(x_near)->bridge = true;
}
tree_vertex_index_t iter = x_near;
while( is_leaf(iter) && get_vertex(iter)->bridge && !is_best_goal(iter))
{
tree_vertex_index_t next = get_vertex(iter)->get_parent();
remove_leaf(iter);
iter = next;
}
}
}
get_vertex(v)->bridge = false;
metric->add_point(tree[v]);
}