42 #include <boost/thread.hpp> 54 InflationLayer::InflationLayer()
55 : inflation_radius_(0)
57 , inflate_unknown_(false)
58 , cell_inflation_radius_(0)
59 , cached_cell_inflation_radius_(0)
63 , cached_distances_(NULL)
64 , last_min_x_(-
std::numeric_limits<float>::
max())
65 , last_min_y_(-
std::numeric_limits<float>::
max())
66 , last_max_x_(
std::numeric_limits<float>::
max())
67 , last_max_y_(
std::numeric_limits<float>::
max())
84 dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig>::CallbackType cb = boost::bind(
88 dsrv_->clearCallback();
89 dsrv_->setCallback(cb);
94 dsrv_->setCallback(cb);
128 double* min_y,
double* max_x,
double* max_y)
139 *min_x = -std::numeric_limits<float>::max();
140 *min_y = -std::numeric_limits<float>::max();
141 *max_x = std::numeric_limits<float>::max();
142 *max_y = std::numeric_limits<float>::max();
169 ROS_DEBUG(
"InflationLayer::onFootprintChanged(): num footprint points: %lu," 170 " inscribed_radius_ = %.3f, inflation_radius_ = %.3f",
183 unsigned char* master_array = master_grid.
getCharMap();
187 ROS_WARN(
"InflationLayer::updateCosts(): seen_ array is NULL");
193 ROS_WARN(
"InflationLayer::updateCosts(): seen_ array size is wrong");
198 memset(
seen_,
false, size_x * size_y *
sizeof(
bool));
209 min_i = std::max(0, min_i);
210 min_j = std::max(0, min_j);
211 max_i = std::min(
int(size_x), max_i);
212 max_j = std::min(
int(size_y), max_j);
219 for (
int j = min_j; j < max_j; j++)
221 for (
int i = min_i; i < max_i; i++)
223 int index = master_grid.
getIndex(i, j);
224 unsigned char cost = master_array[index];
225 if (cost == LETHAL_OBSTACLE)
227 obs_bin.push_back(
CellData(index, i, j, i, j));
234 std::map<double, std::vector<CellData> >::iterator bin;
237 for (
int i = 0; i < bin->second.size(); ++i)
240 const CellData& cell = bin->second[i];
242 unsigned int index = cell.
index_;
252 unsigned int mx = cell.
x_;
253 unsigned int my = cell.
y_;
254 unsigned int sx = cell.
src_x_;
255 unsigned int sy = cell.
src_y_;
258 unsigned char cost =
costLookup(mx, my, sx, sy);
259 unsigned char old_cost = master_array[index];
261 master_array[index] = cost;
263 master_array[index] = std::max(old_cost, cost);
267 enqueue(index - 1, mx - 1, my, sx, sy);
269 enqueue(index - size_x, mx, my - 1, sx, sy);
271 enqueue(index + 1, mx + 1, my, sx, sy);
273 enqueue(index + size_x, mx, my + 1, sx, sy);
290 unsigned int src_x,
unsigned int src_y)
unsigned char computeCost(double distance) const
Given a distance, compute a cost.
LayeredCostmap * layered_costmap_
virtual void onFootprintChanged()
LayeredCostmap calls this whenever the footprint there changes (via LayeredCostmap::setFootprint()). Override to be notified of changes to the robot's footprint.
double ** cached_distances_
PLUGINLIB_EXPORT_CLASS(BAGReader, nodelet::Nodelet)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
std::map< double, std::vector< CellData > > inflation_cells_
static const unsigned char FREE_SPACE
void setInflationParameters(double inflation_radius, double cost_scaling_factor)
Change the values of the inflation radius parameters.
unsigned char * getCharMap() const
Will return a pointer to the underlying unsigned char array used as the costmap.
virtual void onInitialize()
This is called at the end of initialize(). Override to implement subclass-specific initialization...
boost::recursive_mutex * inflation_access_
virtual void matchSize()
Implement this to make this layer match the size of the parent costmap.
virtual void updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j)
Actually update the underlying costmap, only within the bounds calculated during UpdateBounds().
double getInscribedRadius()
The radius of a circle centered at the origin of the robot which is just within all points and edges ...
void enqueue(unsigned int index, unsigned int mx, unsigned int my, unsigned int src_x, unsigned int src_y)
Given an index of a cell in the costmap, place it into a list pending for obstacle inflation...
double distance(double x0, double y0, double x1, double y1)
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
#define ROS_ASSERT_MSG(cond,...)
virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double *min_x, double *min_y, double *max_x, double *max_y)
This is called by the LayeredCostmap to poll this plugin as to how much of the costmap it needs to up...
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
const std::vector< geometry_msgs::Point > & getFootprint()
Returns the latest footprint stored with setFootprint().
void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level)
Storage for cell information used during obstacle inflation.
bool enabled_
Currently this var is managed by subclasses. TODO: make this managed by this class and/or container c...
unsigned char ** cached_costs_
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
static const unsigned char LETHAL_OBSTACLE
static const unsigned char NO_INFORMATION
unsigned int cached_cell_inflation_radius_
unsigned char costLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed costs.
double distanceLookup(int mx, int my, int src_x, int src_y)
Lookup pre-computed distances.
unsigned int cellDistance(double world_dist)
bool need_reinflation_
Indicates that the entire costmap should be reinflated next time around.
A 2D costmap provides a mapping between points in the world and their associated "costs".
dynamic_reconfigure::Server< costmap_2d::InflationPluginConfig > * dsrv_
double getResolution() const
Accessor for the resolution of the costmap.
unsigned int cell_inflation_radius_
double max(double a, double b)
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.