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 TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
00038 #define TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
00039 #include <vector>
00040 #include <list>
00041 #include <cfloat>
00042 #include <geometry_msgs/Point.h>
00043 #include <costmap_2d/observation.h>
00044 #include <base_local_planner/world_model.h>
00045
00046 #include <pcl/point_types.h>
00047 #include <pcl/point_cloud.h>
00048
00049
00050 #include <voxel_grid/voxel_grid.h>
00051
00052 namespace base_local_planner {
00058 class VoxelGridModel : public WorldModel {
00059 public:
00073 VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
00074 double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range);
00075
00079 virtual ~VoxelGridModel(){}
00080
00089 virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
00090 double inscribed_radius, double circumscribed_radius);
00091
00092 using WorldModel::footprintCost;
00093
00100 void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
00101 const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
00102
00103 void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
00104
00105 private:
00114 double lineCost(int x0, int x1, int y0, int y1);
00115
00122 double pointCost(int x, int y);
00123
00124 void removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range);
00125
00126 inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz){
00127 if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
00128 return false;
00129 mx = (int) ((wx - origin_x_) / xy_resolution_);
00130 my = (int) ((wy - origin_y_) / xy_resolution_);
00131 mz = (int) ((wz - origin_z_) / z_resolution_);
00132 return true;
00133 }
00134
00135 inline bool worldToMap2D(double wx, double wy, unsigned int& mx, unsigned int& my){
00136 if(wx < origin_x_ || wy < origin_y_)
00137 return false;
00138 mx = (int) ((wx - origin_x_) / xy_resolution_);
00139 my = (int) ((wy - origin_y_) / xy_resolution_);
00140 return true;
00141 }
00142
00143 inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz){
00144
00145 wx = origin_x_ + (mx + 0.5) * xy_resolution_;
00146 wy = origin_y_ + (my + 0.5) * xy_resolution_;
00147 wz = origin_z_ + (mz + 0.5) * z_resolution_;
00148 }
00149
00150 inline void mapToWorld2D(unsigned int mx, unsigned int my, double& wx, double& wy){
00151
00152 wx = origin_x_ + (mx + 0.5) * xy_resolution_;
00153 wy = origin_y_ + (my + 0.5) * xy_resolution_;
00154 }
00155
00156 inline double dist(double x0, double y0, double z0, double x1, double y1, double z1){
00157 return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
00158 }
00159
00160 inline void insert(pcl::PointXYZ pt){
00161 unsigned int cell_x, cell_y, cell_z;
00162 if(!worldToMap3D(pt.x, pt.y, pt.z, cell_x, cell_y, cell_z))
00163 return;
00164 obstacle_grid_.markVoxel(cell_x, cell_y, cell_z);
00165 }
00166
00167 voxel_grid::VoxelGrid obstacle_grid_;
00168 double xy_resolution_;
00169 double z_resolution_;
00170 double origin_x_;
00171 double origin_y_;
00172 double origin_z_;
00173 double max_z_;
00174 double sq_obstacle_range_;
00175
00176 };
00177 };
00178 #endif