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 
75 double MapGridCostFunction::scoreTrajectory(Trajectory &traj) {
76  double cost = 0.0;
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 */
base_local_planner::Sum
@ Sum
Definition: map_grid_cost_function.h:123
base_local_planner::MapGridCostFunction::getCellCosts
double getCellCosts(unsigned int cx, unsigned int cy)
Definition: map_grid_cost_function.cpp:105
base_local_planner::MapGrid::unreachableCellCosts
double unreachableCellCosts()
Definition: map_grid.h:176
base_local_planner::MapGridCostFunction::stop_on_failure_
bool stop_on_failure_
Definition: map_grid_cost_function.h:170
base_local_planner::MapGridCostFunction::target_poses_
std::vector< geometry_msgs::PoseStamped > target_poses_
Definition: map_grid_cost_function.h:157
costmap_2d::Costmap2D::worldToMap
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
base_local_planner::MapGridCostFunction::yshift_
double yshift_
Definition: map_grid_cost_function.h:167
costmap_2d::Costmap2D
base_local_planner::CostAggregationType
CostAggregationType
Definition: map_grid_cost_function.h:88
base_local_planner::MapGridCostFunction::costmap_
costmap_2d::Costmap2D * costmap_
Definition: map_grid_cost_function.h:158
base_local_planner::MapGridCostFunction::scoreTrajectory
double scoreTrajectory(Trajectory &traj)
Definition: map_grid_cost_function.cpp:110
base_local_planner::MapGridCostFunction::prepare
bool prepare()
Definition: map_grid_cost_function.cpp:94
map_grid_cost_function.h
base_local_planner::MapGridCostFunction::setTargetPoses
void setTargetPoses(std::vector< geometry_msgs::PoseStamped > target_poses)
Definition: map_grid_cost_function.cpp:90
base_local_planner::MapGridCostFunction::aggregationType_
CostAggregationType aggregationType_
Definition: map_grid_cost_function.h:161
base_local_planner::MapGrid::obstacleCosts
double obstacleCosts()
Definition: map_grid.h:168
base_local_planner::MapGrid::setLocalGoal
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
base_local_planner::MapGridCostFunction::MapGridCostFunction
MapGridCostFunction(costmap_2d::Costmap2D *costmap, double xshift=0.0, double yshift=0.0, bool is_local_goal_function=false, CostAggregationType aggregationType=Last)
Definition: map_grid_cost_function.cpp:77
base_local_planner::MapGrid::resetPathDist
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125
ROS_WARN
#define ROS_WARN(...)
base_local_planner::Last
@ Last
Definition: map_grid_cost_function.h:123
base_local_planner::Product
@ Product
Definition: map_grid_cost_function.h:123
base_local_planner::MapGridCostFunction::is_local_goal_function_
bool is_local_goal_function_
Definition: map_grid_cost_function.h:169
base_local_planner::MapGridCostFunction::map_
base_local_planner::MapGrid map_
Definition: map_grid_cost_function.h:160
base_local_planner::MapGrid::setTargetCells
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
base_local_planner
Definition: costmap_model.h:44
base_local_planner::MapGridCostFunction::xshift_
double xshift_
xshift and yshift allow scoring for different
Definition: map_grid_cost_function.h:166


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:24