obstacle_cost_function.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/obstacle_cost_function.h>
00039 #include <cmath>
00040 #include <Eigen/Core>
00041 #include <ros/console.h>
00042 
00043 namespace base_local_planner {
00044 
00045 ObstacleCostFunction::ObstacleCostFunction(costmap_2d::Costmap2D* costmap) 
00046     : costmap_(costmap), sum_scores_(false) {
00047   if (costmap != NULL) {
00048     world_model_ = new base_local_planner::CostmapModel(*costmap_);
00049   }
00050 }
00051 
00052 ObstacleCostFunction::~ObstacleCostFunction() {
00053   if (world_model_ != NULL) {
00054     delete world_model_;
00055   }
00056 }
00057 
00058 
00059 void ObstacleCostFunction::setParams(double max_trans_vel, double max_scaling_factor, double scaling_speed) {
00060   // TODO: move this to prepare if possible
00061   max_trans_vel_ = max_trans_vel;
00062   max_scaling_factor_ = max_scaling_factor;
00063   scaling_speed_ = scaling_speed;
00064 }
00065 
00066 void ObstacleCostFunction::setFootprint(std::vector<geometry_msgs::Point> footprint_spec) {
00067   footprint_spec_ = footprint_spec;
00068 }
00069 
00070 bool ObstacleCostFunction::prepare() {
00071   return true;
00072 }
00073 
00074 double ObstacleCostFunction::scoreTrajectory(Trajectory &traj) {
00075   double cost = 0;
00076   double scale = getScalingFactor(traj, scaling_speed_, max_trans_vel_, max_scaling_factor_);
00077   double px, py, pth;
00078   if (footprint_spec_.size() == 0) {
00079     // Bug, should never happen
00080     ROS_ERROR("Footprint spec is empty, maybe missing call to setFootprint?");
00081     return -9;
00082   }
00083 
00084   for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
00085     traj.getPoint(i, px, py, pth);
00086     double f_cost = footprintCost(px, py, pth,
00087         scale, footprint_spec_,
00088         costmap_, world_model_);
00089 
00090     if(f_cost < 0){
00091         return f_cost;
00092     }
00093 
00094     if(sum_scores_)
00095         cost +=  f_cost;
00096     else
00097         cost = f_cost;
00098   }
00099   return cost;
00100 }
00101 
00102 double ObstacleCostFunction::getScalingFactor(Trajectory &traj, double scaling_speed, double max_trans_vel, double max_scaling_factor) {
00103   double vmag = hypot(traj.xv_, traj.yv_);
00104 
00105   //if we're over a certain speed threshold, we'll scale the robot's
00106   //footprint to make it either slow down or stay further from walls
00107   double scale = 1.0;
00108   if (vmag > scaling_speed) {
00109     //scale up to the max scaling factor linearly... this could be changed later
00110     double ratio = (vmag - scaling_speed) / (max_trans_vel - scaling_speed);
00111     scale = max_scaling_factor * ratio + 1.0;
00112   }
00113   return scale;
00114 }
00115 
00116 double ObstacleCostFunction::footprintCost (
00117     const double& x,
00118     const double& y,
00119     const double& th,
00120     double scale,
00121     std::vector<geometry_msgs::Point> footprint_spec,
00122     costmap_2d::Costmap2D* costmap,
00123     base_local_planner::WorldModel* world_model) {
00124 
00125   //check if the footprint is legal
00126   // TODO: Cache inscribed radius
00127   double footprint_cost = world_model->footprintCost(x, y, th, footprint_spec);
00128 
00129   if (footprint_cost < 0) {
00130     return -6.0;
00131   }
00132   unsigned int cell_x, cell_y;
00133 
00134   //we won't allow trajectories that go off the map... shouldn't happen that often anyways
00135   if ( ! costmap->worldToMap(x, y, cell_x, cell_y)) {
00136     return -7.0;
00137   }
00138 
00139   double occ_cost = std::max(std::max(0.0, footprint_cost), double(costmap->getCost(cell_x, cell_y)));
00140 
00141   return occ_cost;
00142 }
00143 
00144 } /* namespace base_local_planner */


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