map_grid_cost_function.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  *
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2008, Willow Garage, Inc.
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Willow Garage, Inc. nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *
35  * Author: TKruse
36  *********************************************************************/
37 
39 
40 namespace base_local_planner {
41 
43  double xshift,
44  double yshift,
45  bool is_local_goal_function,
46  CostAggregationType aggregationType) :
47  costmap_(costmap),
48  map_(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()),
49  aggregationType_(aggregationType),
50  xshift_(xshift),
51  yshift_(yshift),
52  is_local_goal_function_(is_local_goal_function),
53  stop_on_failure_(true) {}
54 
55 void MapGridCostFunction::setTargetPoses(std::vector<geometry_msgs::PoseStamped> target_poses) {
56  target_poses_ = target_poses;
57 }
58 
61 
64  } else {
66  }
67  return true;
68 }
69 
70 double MapGridCostFunction::getCellCosts(unsigned int px, unsigned int py) {
71  double grid_dist = map_(px, py).target_dist;
72  return grid_dist;
73 }
74 
76  double cost = 0.0;
77  if (aggregationType_ == Product) {
78  cost = 1.0;
79  }
80  double px, py, pth;
81  unsigned int cell_x, cell_y;
82  double grid_dist;
83 
84  for (unsigned int i = 0; i < traj.getPointsSize(); ++i) {
85  traj.getPoint(i, px, py, pth);
86 
87  // translate point forward if specified
88  if (xshift_ != 0.0) {
89  px = px + xshift_ * cos(pth);
90  py = py + xshift_ * sin(pth);
91  }
92  // translate point sideways if specified
93  if (yshift_ != 0.0) {
94  px = px + yshift_ * cos(pth + M_PI_2);
95  py = py + yshift_ * sin(pth + M_PI_2);
96  }
97 
98  //we won't allow trajectories that go off the map... shouldn't happen that often anyways
99  if ( ! costmap_->worldToMap(px, py, cell_x, cell_y)) {
100  //we're off the map
101  ROS_WARN("Off Map %f, %f", px, py);
102  return -4.0;
103  }
104  grid_dist = getCellCosts(cell_x, cell_y);
105  //if a point on this trajectory has no clear path to the goal... it may be invalid
106  if (stop_on_failure_) {
107  if (grid_dist == map_.obstacleCosts()) {
108  return -3.0;
109  } else if (grid_dist == map_.unreachableCellCosts()) {
110  return -2.0;
111  }
112  }
113 
114  switch( aggregationType_ ) {
115  case Last:
116  cost = grid_dist;
117  break;
118  case Sum:
119  cost += grid_dist;
120  break;
121  case Product:
122  if (cost > 0) {
123  cost *= grid_dist;
124  }
125  break;
126  }
127  }
128  return cost;
129 }
130 
131 } /* namespace base_local_planner */
void setTargetCells(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cells are considered path based on the global plan.
Definition: map_grid.cpp:171
double xshift_
xshift and yshift allow scoring for different
MapGridCostFunction(costmap_2d::Costmap2D *costmap, double xshift=0.0, double yshift=0.0, bool is_local_goal_function=false, CostAggregationType aggregationType=Last)
void getPoint(unsigned int index, double &x, double &y, double &th) const
Get a point within the trajectory.
Definition: trajectory.cpp:47
#define ROS_WARN(...)
unsigned int getPointsSize() const
Return the number of points in the trajectory.
Definition: trajectory.cpp:77
void setTargetPoses(std::vector< geometry_msgs::PoseStamped > target_poses)
double getCellCosts(unsigned int cx, unsigned int cy)
void setLocalGoal(const costmap_2d::Costmap2D &costmap, const std::vector< geometry_msgs::PoseStamped > &global_plan)
Update what cell is considered the next local goal.
Definition: map_grid.cpp:210
std::vector< geometry_msgs::PoseStamped > target_poses_
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:44:25