Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #include <base_local_planner/simple_scored_sampling_planner.h>
00039
00040 #include <ros/console.h>
00041
00042 namespace base_local_planner {
00043
00044 SimpleScoredSamplingPlanner::SimpleScoredSamplingPlanner(std::vector<TrajectorySampleGenerator*> gen_list, std::vector<TrajectoryCostFunction*>& critics, int max_samples) {
00045 max_samples_ = max_samples;
00046 gen_list_ = gen_list;
00047 critics_ = critics;
00048 }
00049
00050 double SimpleScoredSamplingPlanner::scoreTrajectory(Trajectory& traj, double best_traj_cost) {
00051 double traj_cost = 0;
00052 int gen_id = 0;
00053 for(std::vector<TrajectoryCostFunction*>::iterator score_function = critics_.begin(); score_function != critics_.end(); ++score_function) {
00054 TrajectoryCostFunction* score_function_p = *score_function;
00055 if (score_function_p->getScale() == 0) {
00056 continue;
00057 }
00058 double cost = score_function_p->scoreTrajectory(traj);
00059 if (cost < 0) {
00060 ROS_DEBUG("Velocity %.3lf, %.3lf, %.3lf discarded by cost function %d with cost: %f", traj.xv_, traj.yv_, traj.thetav_, gen_id, cost);
00061 traj_cost = cost;
00062 break;
00063 }
00064 if (cost != 0) {
00065 cost *= score_function_p->getScale();
00066 }
00067 traj_cost += cost;
00068 if (best_traj_cost > 0) {
00069
00070 if (traj_cost > best_traj_cost) {
00071 break;
00072 }
00073 }
00074 gen_id ++;
00075 }
00076
00077
00078 return traj_cost;
00079 }
00080
00081 bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory& traj, std::vector<Trajectory>* all_explored) {
00082 Trajectory loop_traj;
00083 Trajectory best_traj;
00084 double loop_traj_cost, best_traj_cost = -1;
00085 bool gen_success;
00086 int count, count_valid;
00087 for (std::vector<TrajectoryCostFunction*>::iterator loop_critic = critics_.begin(); loop_critic != critics_.end(); ++loop_critic) {
00088 TrajectoryCostFunction* loop_critic_p = *loop_critic;
00089 if (loop_critic_p->prepare() == false) {
00090 ROS_WARN("A scoring function failed to prepare");
00091 return false;
00092 }
00093 }
00094
00095 for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
00096 count = 0;
00097 count_valid = 0;
00098 TrajectorySampleGenerator* gen_ = *loop_gen;
00099 while (gen_->hasMoreTrajectories()) {
00100 gen_success = gen_->nextTrajectory(loop_traj);
00101 if (gen_success == false) {
00102
00103 continue;
00104 }
00105 loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
00106 if (all_explored != NULL) {
00107 loop_traj.cost_ = loop_traj_cost;
00108 all_explored->push_back(loop_traj);
00109 }
00110
00111 if (loop_traj_cost >= 0) {
00112 count_valid++;
00113 if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
00114 best_traj_cost = loop_traj_cost;
00115 best_traj = loop_traj;
00116 }
00117 }
00118 count++;
00119 if (max_samples_ > 0 && count >= max_samples_) {
00120 break;
00121 }
00122 }
00123 if (best_traj_cost >= 0) {
00124 traj.xv_ = best_traj.xv_;
00125 traj.yv_ = best_traj.yv_;
00126 traj.thetav_ = best_traj.thetav_;
00127 traj.cost_ = best_traj_cost;
00128 traj.resetPoints();
00129 double px, py, pth;
00130 for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
00131 best_traj.getPoint(i, px, py, pth);
00132 traj.addPoint(px, py, pth);
00133 }
00134 }
00135 ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
00136 if (best_traj_cost >= 0) {
00137
00138 break;
00139 }
00140 }
00141 return best_traj_cost >= 0;
00142 }
00143
00144
00145 }