simple_scored_sampling_planner.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2008, Willow Garage, Inc.
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/or other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of Willow Garage, Inc. nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *
00035  * Author: TKruse
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         // since we keep adding positives, once we are worse than the best, we will stay worse
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           // TODO use this for debugging
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         // do not try fallback generators
00138         break;
00139       }
00140     }
00141     return best_traj_cost >= 0;
00142   }
00143 
00144   
00145 }// namespace


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Aug 27 2015 14:07:08