src/smp/planners/rrtstar.h

Go to the documentation of this file.

#ifndef _SMP_PLANNER_RRTSTAR_H_
#define _SMP_PLANNER_RRTSTAR_H_

#include <smp/planners/base_incremental.h>
#include <smp/planners/planner_parameters.h>

#include <smp/components/cost_evaluators/base.h>


namespace smp {


    class rrtstar_vertex_data {
        
    public:
        double total_cost;
    };



    class rrtstar_edge_data {
        
    public:
        double edge_cost;
    };
    
    

    template< class typeparams >
    class rrtstar : public planner_incremental<typeparams> {


        typedef typename typeparams::state state_t;
        typedef typename typeparams::input input_t;
        typedef typename typeparams::vertex_data vertex_data_t;
        typedef typename typeparams::edge_data edge_data_t;

        typedef trajectory<typeparams> trajectory_t;

        typedef vertex<typeparams> vertex_t;
        typedef smp::edge<typeparams> edge_t;

        typedef planner_incremental<typeparams> planner_incremental_t;

        typedef cost_evaluator_base<typeparams> cost_evaluator_t;
    
        typedef planner_parameters parameters_t;

        typedef sampler_base<typeparams> sampler_t;
        typedef distance_evaluator_base<typeparams> distance_evaluator_t;
        typedef extender_base<typeparams> extender_t;
        typedef collision_checker_base<typeparams> collision_checker_t;
        typedef model_checker_base<typeparams> model_checker_t;


    private:


        // This function adds the given state to the beginning of the tracjetory and calls the collision checker.
        int check_extended_trajectory_for_collision (state_t *state, trajectory_t *trajectory) {

            trajectory->list_states.push_front (state);
            int collision_check = this->collision_checker.check_collision_trajectory (trajectory);
            trajectory->list_states.pop_front ();

            return collision_check;
        }

        // The radius that was used in the previous iteration
        double radius_last; 

    
    protected:


    
    

        cost_evaluator_t &cost_evaluator;




        int propagate_cost (vertex_t *vertex_in, double total_cost_new);
    
    
    public:



        parameters_t parameters;


        rrtstar ();
        ~rrtstar ();


        rrtstar (sampler_t &sampler_in, distance_evaluator_t &distance_evaluator_in, 
                 extender_t &extender_in, collision_checker_t &collision_checker_in, 
                 model_checker_t &model_checker_in, cost_evaluator_t &cost_evaluator_in);


        int initialize (state_t *initial_state_in = 0);


    
        int init_cost_evaluator (cost_evaluator_t &cost_evaluator_in);


    
        double get_ball_radius_last () {return radius_last;}


        int iteration ();

    };


}

#endif