00001 00010 #ifndef _SMP_MINIMUM_TIME_REACHABILITY_H_ 00011 #define _SMP_MINIMUM_TIME_REACHABILITY_H_ 00012 00013 #include <smp/planners/rrtstar.h> 00014 #include <smp/common/region.h> 00015 #include <smp/components/model_checkers/base.h> 00016 #include <smp/components/cost_evaluators/base.h> 00017 00018 00019 00020 namespace smp { 00021 00022 00024 00031 class minimum_time_reachability_vertex_data : public rrtstar_vertex_data { 00032 00033 public: 00034 00036 00040 bool reaches_goal; 00041 }; 00042 00043 00044 00046 00049 class minimum_time_reachability_edge_data : public rrtstar_edge_data { 00050 00051 }; 00052 00053 00054 00056 00064 template< class typeparams, int NUM_DIMENSIONS > 00065 class minimum_time_reachability : public model_checker_base<typeparams> , public cost_evaluator_base<typeparams> { 00066 00067 00068 typedef typename typeparams::state state_t; 00069 typedef typename typeparams::input input_t; 00070 typedef typename typeparams::vertex_data vertex_data_t; 00071 typedef typename typeparams::edge_data edge_data_t; 00072 00073 typedef vertex<typeparams> vertex_t; 00074 typedef edge<typeparams> edge_t; 00075 typedef trajectory<typeparams> trajectory_t; 00076 00077 typedef region<NUM_DIMENSIONS> region_t; 00078 00079 typedef int (*update_func_t)(trajectory_t *); 00080 00081 00082 list<update_func_t> list_update_functions; // A list of functions that will be called in the 00083 // event of updating the minimum cost trajectory. 00084 00085 00086 vertex_t *min_cost_vertex; // A pointer to the minimum cost vertex in the tree 00087 trajectory_t min_cost_trajectory; // A copy of the mininum cost trajectory 00088 00089 00090 region_t region_goal; 00091 00092 00093 public: 00094 00095 minimum_time_reachability (); 00096 ~minimum_time_reachability (); 00097 00109 minimum_time_reachability (const region_t ®ion_goal); 00110 00121 int set_goal_region (const region_t ®ion_goal); 00122 00123 int ce_update_vertex_cost (vertex_t *vertex_in); 00124 00125 int ce_update_edge_cost (edge_t *edge_in); 00126 00127 int mc_update_insert_vertex (vertex_t *vertex_in); 00128 00129 int mc_update_insert_edge (edge_t *edge_in); 00130 00131 int mc_update_delete_vertex (vertex_t *vertex_in); 00132 00133 int mc_update_delete_edge (edge_t *edge_in); 00134 00135 int get_solution (trajectory_t &trajectory_out); 00136 00137 double evaluate_cost_trajectory (state_t *state_initial_in, 00138 trajectory_t *trajectory_in, 00139 state_t *state_final_in = 0); 00140 00141 00150 double get_best_cost (); 00151 00161 int clear_update_function_list (); 00162 00173 int register_new_update_function (update_func_t update_function); 00174 00175 }; 00176 00177 00178 } 00179 00180 #endif
