costmap_model.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 the Willow Garage 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: Eitan Marder-Eppstein
36 *********************************************************************/
39 #include <costmap_2d/cost_values.h>
40 
41 using namespace std;
42 using namespace costmap_2d;
43 
44 namespace base_local_planner {
45  CostmapModel::CostmapModel(const Costmap2D& ma) : costmap_(ma) {}
46 
47  double CostmapModel::footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
48  double inscribed_radius, double circumscribed_radius){
49  // returns:
50  // -1 if footprint covers at least a lethal obstacle cell, or
51  // -2 if footprint covers at least a no-information cell, or
52  // -3 if footprint is [partially] outside of the map, or
53  // a positive value for traversable space
54 
55  //used to put things into grid coordinates
56  unsigned int cell_x, cell_y;
57 
58  //get the cell coord of the center point of the robot
59  if(!costmap_.worldToMap(position.x, position.y, cell_x, cell_y))
60  return -3.0;
61 
62  //if number of points in the footprint is less than 3, we'll just assume a circular robot
63  if(footprint.size() < 3){
64  unsigned char cost = costmap_.getCost(cell_x, cell_y);
65  if(cost == NO_INFORMATION)
66  return -2.0;
67  if(cost == LETHAL_OBSTACLE || cost == INSCRIBED_INFLATED_OBSTACLE)
68  return -1.0;
69  return cost;
70  }
71 
72  //now we really have to lay down the footprint in the costmap grid
73  unsigned int x0, x1, y0, y1;
74  double line_cost = 0.0;
75  double footprint_cost = 0.0;
76 
77  //we need to rasterize each line in the footprint
78  for(unsigned int i = 0; i < footprint.size() - 1; ++i){
79  //get the cell coord of the first point
80  if(!costmap_.worldToMap(footprint[i].x, footprint[i].y, x0, y0))
81  return -3.0;
82 
83  //get the cell coord of the second point
84  if(!costmap_.worldToMap(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
85  return -3.0;
86 
87  line_cost = lineCost(x0, x1, y0, y1);
88  footprint_cost = std::max(line_cost, footprint_cost);
89 
90  //if there is an obstacle that hits the line... we know that we can return false right away
91  if(line_cost < 0)
92  return line_cost;
93  }
94 
95  //we also need to connect the first point in the footprint to the last point
96  //get the cell coord of the last point
97  if(!costmap_.worldToMap(footprint.back().x, footprint.back().y, x0, y0))
98  return -3.0;
99 
100  //get the cell coord of the first point
101  if(!costmap_.worldToMap(footprint.front().x, footprint.front().y, x1, y1))
102  return -3.0;
103 
104  line_cost = lineCost(x0, x1, y0, y1);
105  footprint_cost = std::max(line_cost, footprint_cost);
106 
107  if(line_cost < 0)
108  return line_cost;
109 
110  //if all line costs are legal... then we can return that the footprint is legal
111  return footprint_cost;
112 
113  }
114 
115  //calculate the cost of a ray-traced line
116  double CostmapModel::lineCost(int x0, int x1, int y0, int y1) const {
117  double line_cost = 0.0;
118  double point_cost = -1.0;
119 
120  for( LineIterator line( x0, y0, x1, y1 ); line.isValid(); line.advance() )
121  {
122  point_cost = pointCost( line.getX(), line.getY() ); //Score the current point
123 
124  if(point_cost < 0)
125  return point_cost;
126 
127  if(line_cost < point_cost)
128  line_cost = point_cost;
129  }
130 
131  return line_cost;
132  }
133 
134  double CostmapModel::pointCost(int x, int y) const {
135  unsigned char cost = costmap_.getCost(x, y);
136  //if the cell is in an obstacle the path is invalid
137  if(cost == NO_INFORMATION)
138  return -2;
139  if(cost == LETHAL_OBSTACLE)
140  return -1;
141 
142  return cost;
143  }
144 
145 };
const costmap_2d::Costmap2D & costmap_
Allows access of costmap obstacle information.
Definition: costmap_model.h:98
double pointCost(int x, int y) const
Checks the cost of a point in the costmap.
unsigned char getCost(unsigned int mx, unsigned int my) const
double lineCost(int x0, int x1, int y0, int y1) const
Rasterizes a line in the costmap grid and checks for collisions.
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
Checks if any obstacles in the costmap lie inside a convex footprint that is rasterized into the grid...
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49