first commit

include/costmap_2d/inflation_layer.h

@@ -161,4 +161,107 @@
 
 // }  // namespace costmap_2d
 
-#endif  // COSTMAP_2D_INFLATION_LAYER_H_
+// #endif  // COSTMAP_2D_INFLATION_LAYER_H_
+
+
+
+
+
+
+
+
+
+
+
+
+
+// /////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+// #ifndef INFLATION_LAYER_H_
+// #define INFLATION_LAYER_H_
+
+// #include "share.h"
+// #include <cmath>
+// #include <map>
+// #include <vector>
+// #include <boost/thread/recursive_mutex.hpp>
+
+// /**
+//  * @brief Lưu thông tin 1 cell khi thực hiện inflation
+//  */
+// class CellData
+// {
+// public:
+//     CellData(double i, unsigned int x, unsigned int y, unsigned int sx, unsigned int sy)
+//         : index_(i), x_(x), y_(y), src_x_(sx), src_y_(sy) {}
+
+//     unsigned int index_;
+//     unsigned int x_, y_;
+//     unsigned int src_x_, src_y_;
+// };
+
+// /**
+//  * @brief Lớp thực hiện "inflation" cho costmap (mở rộng vùng vật cản)
+//  */
+// class InflationLayer : public Layer
+// {
+// public:
+//     InflationLayer();
+//     virtual ~InflationLayer();
+
+//     virtual void onInitialize();
+//     virtual void updateBounds(double robot_x, double robot_y, double robot_yaw,
+//                               double* min_x, double* min_y, double* max_x, double* max_y);
+//     virtual void updateCosts(Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+//     virtual bool isDiscretized() { return true; }
+//     virtual void matchSize();
+//     virtual void reset() { onInitialize(); }
+
+//     void setInflationParameters(double inflation_radius, double cost_scaling_factor);
+
+// protected:
+//     virtual void onFootprintChanged();
+
+//     boost::recursive_mutex inflation_access_;
+
+//     double resolution_;
+//     double inflation_radius_;
+//     double inscribed_radius_;
+//     double weight_;
+//     bool inflate_unknown_;
+
+// private:
+//     inline double distanceLookup(int mx, int my, int src_x, int src_y);
+//     inline unsigned char costLookup(int mx, int my, int src_x, int src_y);
+//     inline unsigned char computeCost(double distance) const;
+
+//     void computeCaches();
+//     void deleteKernels();
+//     void inflate_area(int min_i, int min_j, int max_i, int max_j, unsigned char* master_grid);
+//     inline void enqueue(unsigned int index, unsigned int mx, unsigned int my,
+//                         unsigned int src_x, unsigned int src_y);
+
+//     unsigned int cell_inflation_radius_;
+//     unsigned int cached_cell_inflation_radius_;
+//     std::map<double, std::vector<CellData>> inflation_cells_;
+
+//     bool* seen_;
+//     int seen_size_;
+
+//     unsigned char** cached_costs_;
+//     double** cached_distances_;
+//     double last_min_x_, last_min_y_, last_max_x_, last_max_y_;
+
+//     bool need_reinflation_;
+// };
+
+#endif // INFLATION_LAYER_H_
+