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 #ifndef COSTMAP_VOXEL_COSTMAP_2D_H_
00038 #define COSTMAP_VOXEL_COSTMAP_2D_H_
00039
00040 #include <vector>
00041 #include <queue>
00042 #include <costmap_2d/costmap_2d.h>
00043 #include <costmap_2d/observation.h>
00044 #include <costmap_2d/cell_data.h>
00045 #include <costmap_2d/cost_values.h>
00046 #include <voxel_grid/voxel_grid.h>
00047 #include <costmap_2d/VoxelGrid.h>
00048
00049 #include <costmap_2d/Costmap2DConfig.h>
00050
00051 namespace costmap_2d {
00056 class VoxelCostmap2D : public Costmap2D {
00057 public:
00081 VoxelCostmap2D(unsigned int cells_size_x, unsigned int cells_size_y, unsigned int cells_size_z,
00082 double xy_resolution, double z_resolution, double origin_x, double origin_y, double origin_z = 0.0, double inscribed_radius = 0.0,
00083 double circumscribed_radius = 0.0, double inflation_radius = 0.0, double obstacle_range = 0.0,
00084 double raytrace_range = 0.0, double weight = 25.0,
00085 const std::vector<unsigned char>& static_data = std::vector<unsigned char>(0), unsigned char lethal_threshold = 0,
00086 unsigned int unknown_threshold = 0, unsigned int mark_threshold = 0, unsigned char unknown_cost_value = 0);
00087
00088
00089 VoxelCostmap2D(costmap_2d::Costmap2D& costmap,
00090 double z_resolution, unsigned int cells_size_z, double origin_z=0.0,
00091 unsigned int mark_threshold=0, unsigned int unknown_threshold=0);
00092
00096 ~VoxelCostmap2D();
00097
00105 void resetMapOutsideWindow(double wx, double wy, double w_size_x, double w_size_y);
00106
00112 void updateOrigin(double new_origin_x, double new_origin_y);
00113
00122 virtual void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info = false);
00123
00128 void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
00129
00130 void getVoxelGridMessage(VoxelGrid& grid);
00131
00132 static inline void mapToWorld3D(const unsigned int mx, const unsigned int my, const unsigned int mz,
00133 const double origin_x, const double origin_y, const double origin_z,
00134 const double x_resolution, const double y_resolution, const double z_resolution,
00135 double& wx, double& wy, double& wz){
00136
00137 wx = origin_x + (mx + 0.5) * x_resolution;
00138 wy = origin_y + (my + 0.5) * y_resolution;
00139 wz = origin_z + (mz + 0.5) * z_resolution;
00140 }
00141
00142 protected:
00146 virtual void resetMaps();
00147
00153 virtual void initMaps(unsigned int size_x, unsigned int size_y);
00154
00155
00156 private:
00162 void updateObstacles(const std::vector<Observation>& observations, std::priority_queue<CellData>& inflation_queue);
00163
00168 void raytraceFreespace(const Observation& clearing_observation);
00169
00170 inline bool worldToMap3DFloat(double wx, double wy, double wz, double& mx, double& my, double& mz){
00171 if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
00172 return false;
00173 mx = ((wx - origin_x_) / xy_resolution_);
00174 my = ((wy - origin_y_) / xy_resolution_);
00175 mz = ((wz - origin_z_) / z_resolution_);
00176
00177 if(mx < size_x_ && my < size_y_ && mz < size_z_)
00178 return true;
00179
00180 return false;
00181 }
00182
00183 inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz){
00184 if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
00185 return false;
00186
00187 mx = (int) ((wx - origin_x_) / xy_resolution_);
00188 my = (int) ((wy - origin_y_) / xy_resolution_);
00189 mz = (int) ((wz - origin_z_) / z_resolution_);
00190
00191 if(mx < size_x_ && my < size_y_ && mz < size_z_)
00192 return true;
00193
00194 return false;
00195 }
00196
00197 inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz){
00198
00199 wx = origin_x_ + (mx + 0.5) * xy_resolution_;
00200 wy = origin_y_ + (my + 0.5) * xy_resolution_;
00201 wz = origin_z_ + (mz + 0.5) * z_resolution_;
00202 }
00203
00204 inline double dist(double x0, double y0, double z0, double x1, double y1, double z1){
00205 return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
00206 }
00207
00208 void finishConfiguration(costmap_2d::Costmap2DConfig &config);
00209
00210 protected:
00211 voxel_grid::VoxelGrid voxel_grid_;
00212 double xy_resolution_, z_resolution_, origin_z_;
00213 unsigned int unknown_threshold_, mark_threshold_, size_z_;
00214
00215 };
00216 };
00217
00218 #endif