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


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko
autogenerated on Mon Oct 6 2014 02:45:34