Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef COSTMAP_2D_INFLATION_LAYER_H_
00039 #define COSTMAP_2D_INFLATION_LAYER_H_
00040
00041 #include <ros/ros.h>
00042 #include <costmap_2d/layer.h>
00043 #include <costmap_2d/layered_costmap.h>
00044 #include <costmap_2d/InflationPluginConfig.h>
00045 #include <dynamic_reconfigure/server.h>
00046 #include <boost/thread.hpp>
00047
00048 namespace costmap_2d
00049 {
00054 class CellData
00055 {
00056 public:
00066 CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
00067 index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
00068 {
00069 }
00070 unsigned int index_;
00071 unsigned int x_, y_;
00072 unsigned int src_x_, src_y_;
00073 };
00074
00075 class InflationLayer : public Layer
00076 {
00077 public:
00078 InflationLayer();
00079
00080 virtual ~InflationLayer()
00081 {
00082 deleteKernels();
00083 if (dsrv_)
00084 delete dsrv_;
00085 }
00086
00087 virtual void onInitialize();
00088 virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00089 double* max_x, double* max_y);
00090 virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00091 virtual bool isDiscretized()
00092 {
00093 return true;
00094 }
00095 virtual void matchSize();
00096
00097 virtual void reset() { onInitialize(); }
00098
00102 inline unsigned char computeCost(double distance) const
00103 {
00104 unsigned char cost = 0;
00105 if (distance == 0)
00106 cost = LETHAL_OBSTACLE;
00107 else if (distance * resolution_ <= inscribed_radius_)
00108 cost = INSCRIBED_INFLATED_OBSTACLE;
00109 else
00110 {
00111
00112 double euclidean_distance = distance * resolution_;
00113 double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
00114 cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
00115 }
00116 return cost;
00117 }
00118
00124 void setInflationParameters(double inflation_radius, double cost_scaling_factor);
00125
00126 protected:
00127 virtual void onFootprintChanged();
00128 boost::recursive_mutex* inflation_access_;
00129
00130 private:
00139 inline double distanceLookup(int mx, int my, int src_x, int src_y)
00140 {
00141 unsigned int dx = abs(mx - src_x);
00142 unsigned int dy = abs(my - src_y);
00143 return cached_distances_[dx][dy];
00144 }
00145
00154 inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
00155 {
00156 unsigned int dx = abs(mx - src_x);
00157 unsigned int dy = abs(my - src_y);
00158 return cached_costs_[dx][dy];
00159 }
00160
00161 void computeCaches();
00162 void deleteKernels();
00163 void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
00164
00165 unsigned int cellDistance(double world_dist)
00166 {
00167 return layered_costmap_->getCostmap()->cellDistance(world_dist);
00168 }
00169
00170 inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
00171 unsigned int src_x, unsigned int src_y);
00172
00173 double inflation_radius_, inscribed_radius_, weight_;
00174 unsigned int cell_inflation_radius_;
00175 unsigned int cached_cell_inflation_radius_;
00176 std::map<double, std::vector<CellData> > inflation_cells_;
00177
00178 double resolution_;
00179
00180 bool* seen_;
00181 int seen_size_;
00182
00183 unsigned char** cached_costs_;
00184 double** cached_distances_;
00185 double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
00186
00187 dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;
00188 void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);
00189
00190 bool need_reinflation_;
00191 };
00192
00193 }
00194
00195 #endif // COSTMAP_2D_INFLATION_LAYER_H_
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Sun Mar 3 2019 03:46:15