map_grid_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/map_grid_cost_function.h>
00039 
00040 namespace base_local_planner {
00041 
00042 MapGridCostFunction::MapGridCostFunction(costmap_2d::Costmap2D* costmap,
00043     double xshift,
00044     double yshift,
00045     bool is_local_goal_function,
00046     CostAggregationType aggregationType) :
00047     costmap_(costmap),
00048     map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()),
00049     aggregationType_(aggregationType),
00050     xshift_(xshift),
00051     yshift_(yshift),
00052     is_local_goal_function_(is_local_goal_function),
00053     stop_on_failure_(true) {}
00054 
00055 void MapGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
00056   target_poses_ = target_poses;
00057 }
00058 
00059 bool MapGridCostFunction::prepare() {
00060   map_.resetPathDist();
00061 
00062   if (is_local_goal_function_) {
00063     map_.setLocalGoal(*costmap_, target_poses_);
00064   } else {
00065     map_.setTargetCells(*costmap_, target_poses_);
00066   }
00067   return true;
00068 }
00069 
00070 double MapGridCostFunction::getCellCosts(unsigned int px, unsigned int py) {
00071   double grid_dist = map_(px, py).target_dist;
00072   return grid_dist;
00073 }
00074 
00075 double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
00076   double cost = 0.0;
00077   if (aggregationType_ == Product) {
00078     cost = 1.0;
00079   }
00080   double px, py, pth;
00081   unsigned int cell_x, cell_y;
00082   double grid_dist;
00083 
00084   for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
00085     traj.getPoint(i, px, py, pth);
00086 
00087     // translate point forward if specified
00088     if (xshift_ != 0.0) {
00089       px = px + xshift_ * cos(pth);
00090       py = py + xshift_ * sin(pth);
00091     }
00092     // translate point sideways if specified
00093     if (yshift_ != 0.0) {
00094       px = px + yshift_ * cos(pth + M_PI_2);
00095       py = py + yshift_ * sin(pth + M_PI_2);
00096     }
00097 
00098     //we won't allow trajectories that go off the map... shouldn't happen that often anyways
00099     if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
00100       //we're off the map
00101       ROS_WARN("Off Map %f, %f", px, py);
00102       return -4.0;
00103     }
00104     grid_dist = getCellCosts(cell_x, cell_y);
00105     //if a point on this trajectory has no clear path to the goal... it may be invalid
00106     if (stop_on_failure_) {
00107       if (grid_dist == map_.obstacleCosts()) {
00108         return -3.0;
00109       } else if (grid_dist == map_.unreachableCellCosts()) {
00110         return -2.0;
00111       }
00112     }
00113 
00114     switch( aggregationType_ ) {
00115     case Last:
00116       cost = grid_dist;
00117       break;
00118     case Sum:
00119       cost += grid_dist;
00120       break;
00121     case Product:
00122       if (cost > 0) {
00123         cost *= grid_dist;
00124       }
00125       break;
00126     }
00127   }
00128   return cost;
00129 }
00130 
00131 } /* 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