38 #ifndef COSTMAP_2D_INFLATION_LAYER_H_ 39 #define COSTMAP_2D_INFLATION_LAYER_H_ 44 #include <costmap_2d/InflationPluginConfig.h> 45 #include <dynamic_reconfigure/server.h> 46 #include <boost/thread.hpp> 66 CellData(
double i,
unsigned int x,
unsigned int y,
unsigned int sx,
unsigned int sy) :
87 virtual void onInitialize();
88 virtual void updateBounds(
double robot_x,
double robot_y,
double robot_yaw,
double* min_x,
double* min_y,
89 double* max_x,
double* max_y);
90 virtual void updateCosts(
costmap_2d::Costmap2D& master_grid,
int min_i,
int min_j,
int max_i,
int max_j);
95 virtual void matchSize();
97 virtual void reset() { onInitialize(); }
104 unsigned char cost = 0;
107 else if (distance * resolution_ <= inscribed_radius_)
112 double euclidean_distance = distance * resolution_;
113 double factor =
exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
124 void setInflationParameters(
double inflation_radius,
double cost_scaling_factor);
127 virtual void onFootprintChanged();
141 unsigned int dx =
abs(mx - src_x);
142 unsigned int dy =
abs(my - src_y);
143 return cached_distances_[dx][dy];
154 inline unsigned char costLookup(
int mx,
int my,
int src_x,
int src_y)
156 unsigned int dx =
abs(mx - src_x);
157 unsigned int dy =
abs(my - src_y);
158 return cached_costs_[dx][dy];
161 void computeCaches();
162 void deleteKernels();
163 void inflate_area(
int min_i,
int min_j,
int max_i,
int max_j,
unsigned char* master_grid);
167 return layered_costmap_->getCostmap()->cellDistance(world_dist);
170 inline void enqueue(
unsigned int index,
unsigned int mx,
unsigned int my,
171 unsigned int src_x,
unsigned int src_y);
173 double inflation_radius_, inscribed_radius_,
weight_;
188 dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *
dsrv_;
189 void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);
196 #endif // COSTMAP_2D_INFLATION_LAYER_H_
unsigned char computeCost(double distance) const
Given a distance, compute a cost.
double ** cached_distances_
std::map< double, std::vector< CellData > > inflation_cells_
boost::recursive_mutex * inflation_access_
double distance(double x0, double y0, double x1, double y1)
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
Constructor for a CellData objects.
virtual bool isDiscretized()
INLINE Rall1d< T, V, S > exp(const Rall1d< T, V, S > &arg)
Storage for cell information used during obstacle inflation.
unsigned char ** cached_costs_
static const unsigned char LETHAL_OBSTACLE
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
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_
unsigned int cell_inflation_radius_
virtual ~InflationLayer()