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 #include <queue>
00048
00049 namespace costmap_2d
00050 {
00055 class CellData
00056 {
00057 public:
00068 CellData(double d, double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy) :
00069 distance_(d), index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy)
00070 {
00071 }
00072 double distance_;
00073 unsigned int index_;
00074 unsigned int x_, y_;
00075 unsigned int src_x_, src_y_;
00076 };
00077
00082 inline bool operator<(const CellData &a, const CellData &b)
00083 {
00084 return a.distance_ > b.distance_;
00085 }
00086
00087 class InflationLayer : public Layer
00088 {
00089 public:
00090 InflationLayer();
00091
00092 virtual ~InflationLayer()
00093 {
00094 deleteKernels();
00095 if (dsrv_)
00096 delete dsrv_;
00097 }
00098
00099 virtual void onInitialize();
00100 virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
00101 double* max_x, double* max_y);
00102 virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
00103 virtual bool isDiscretized()
00104 {
00105 return true;
00106 }
00107 virtual void matchSize();
00108
00109 virtual void reset() { onInitialize(); }
00110
00114 inline unsigned char computeCost(double distance) const
00115 {
00116 unsigned char cost = 0;
00117 if (distance == 0)
00118 cost = LETHAL_OBSTACLE;
00119 else if (distance * resolution_ <= inscribed_radius_)
00120 cost = INSCRIBED_INFLATED_OBSTACLE;
00121 else
00122 {
00123
00124 double euclidean_distance = distance * resolution_;
00125 double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
00126 cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
00127 }
00128 return cost;
00129 }
00130
00136 void setInflationParameters(double inflation_radius, double cost_scaling_factor);
00137
00138 protected:
00139 virtual void onFootprintChanged();
00140 boost::recursive_mutex* inflation_access_;
00141
00142 private:
00151 inline double distanceLookup(int mx, int my, int src_x, int src_y)
00152 {
00153 unsigned int dx = abs(mx - src_x);
00154 unsigned int dy = abs(my - src_y);
00155 return cached_distances_[dx][dy];
00156 }
00157
00166 inline unsigned char costLookup(int mx, int my, int src_x, int src_y)
00167 {
00168 unsigned int dx = abs(mx - src_x);
00169 unsigned int dy = abs(my - src_y);
00170 return cached_costs_[dx][dy];
00171 }
00172
00173 void computeCaches();
00174 void deleteKernels();
00175 void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
00176
00177 unsigned int cellDistance(double world_dist)
00178 {
00179 return layered_costmap_->getCostmap()->cellDistance(world_dist);
00180 }
00181
00182 inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
00183 unsigned int src_x, unsigned int src_y);
00184
00185 double inflation_radius_, inscribed_radius_, weight_;
00186 unsigned int cell_inflation_radius_;
00187 unsigned int cached_cell_inflation_radius_;
00188 std::priority_queue<CellData> inflation_queue_;
00189
00190 double resolution_;
00191
00192 bool* seen_;
00193 int seen_size_;
00194
00195 unsigned char** cached_costs_;
00196 double** cached_distances_;
00197 double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
00198
00199 dynamic_reconfigure::Server<costmap_2d::InflationPluginConfig> *dsrv_;
00200 void reconfigureCB(costmap_2d::InflationPluginConfig &config, uint32_t level);
00201
00202 bool need_reinflation_;
00203 };
00204
00205 }
00206
00207 #endif // COSTMAP_2D_INFLATION_LAYER_H_
costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Wed Aug 2 2017 03:12:21