43 VoxelGridModel::VoxelGridModel(
double size_x,
double size_y,
double size_z,
double xy_resolution,
double z_resolution,
44 double origin_x,
double origin_y,
double origin_z,
double max_z,
double obstacle_range) :
45 obstacle_grid_(size_x, size_y, size_z), xy_resolution_(xy_resolution), z_resolution_(z_resolution),
46 origin_x_(origin_x), origin_y_(origin_y), origin_z_(origin_z),
47 max_z_(max_z), sq_obstacle_range_(obstacle_range * obstacle_range) {}
50 double inscribed_radius,
double circumscribed_radius){
51 if(footprint.size() < 3)
55 unsigned int x0, x1, y0, y1;
56 double line_cost = 0.0;
59 for(
unsigned int i = 0; i < footprint.size() - 1; ++i){
65 if(!
worldToMap2D(footprint[i + 1].x, footprint[i + 1].y, x1, y1))
68 line_cost =
lineCost(x0, x1, y0, y1);
77 if(!
worldToMap2D(footprint.back().x, footprint.back().y, x0, y0))
81 if(!
worldToMap2D(footprint.front().x, footprint.front().y, x1, y1))
84 line_cost =
lineCost(x0, x1, y0, y1);
97 int deltax =
abs(x1 - x0);
98 int deltay =
abs(y1 - y0);
102 int xinc1, xinc2, yinc1, yinc2;
103 int den, num, numadd, numpixels;
105 double line_cost = 0.0;
106 double point_cost = -1.0;
130 if (deltax >= deltay)
149 for (
int curpixel = 0; curpixel <= numpixels; curpixel++)
156 if(line_cost < point_cost)
157 line_cost = point_cost;
183 const vector<Observation>& observations,
const vector<PlanarLaserScan>& laser_scans){
186 for(
unsigned int i = 0; i < laser_scans.size(); ++i)
190 for(vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
192 const pcl::PointCloud<pcl::PointXYZ>& cloud = *(obs.
cloud_);
193 for(
unsigned int i = 0; i < cloud.size(); ++i){
199 double sq_dist = (cloud[i].x - obs.
origin_.x) * (cloud[i].
x - obs.
origin_.x)
216 if(laser_scan.
cloud.points.size() == 0)
219 unsigned int sensor_x, sensor_y, sensor_z;
220 double ox = laser_scan.
origin.x;
221 double oy = laser_scan.
origin.y;
222 double oz = laser_scan.
origin.z;
224 if(!
worldToMap3D(ox, oy, oz, sensor_x, sensor_y, sensor_z))
227 for(
unsigned int i = 0; i < laser_scan.
cloud.points.size(); ++i){
228 double wpx = laser_scan.
cloud.points[i].x;
229 double wpy = laser_scan.
cloud.points[i].y;
230 double wpz = laser_scan.
cloud.points[i].z;
233 double scaling_fact = raytrace_range / distance;
234 scaling_fact = scaling_fact > 1.0 ? 1.0 : scaling_fact;
235 wpx = scaling_fact * (wpx - ox) + ox;
236 wpy = scaling_fact * (wpy - oy) + oy;
237 wpz = scaling_fact * (wpz - oz) + oz;
245 double t = (
max_z_ - .01 - oz) / c;
256 double t = (0.0 - oz) / c;
262 unsigned int point_x, point_y, point_z;
263 if(
worldToMap3D(wpx, wpy, wpz, point_x, point_y, point_z)){
280 cloud.points.push_back(pt);
double sq_obstacle_range_
The square distance at which we no longer add obstacles to the grid.
geometry_msgs::Point origin_
double pointCost(int x, int y)
Checks the cost of a point in the costmap.
VoxelStatus getVoxelColumn(unsigned int x, unsigned int y, unsigned int unknown_threshold=0, unsigned int marked_threshold=0)
pcl::PointCloud< pcl::PointXYZ > * cloud_
bool worldToMap3D(double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
VoxelStatus getVoxel(unsigned int x, unsigned int y, unsigned int z)
TFSIMD_FORCE_INLINE const tfScalar & y() const
geometry_msgs::TransformStamped t
geometry_msgs::Point32 origin
void updateWorld(const std::vector< geometry_msgs::Point > &footprint, const std::vector< costmap_2d::Observation > &observations, const std::vector< PlanarLaserScan > &laser_scans)
The costmap already keeps track of world observations, so for this world model this method does nothi...
double distance(double x0, double y0, double x1, double y1)
void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double &wx, double &wy, double &wz)
double lineCost(int x0, int x1, int y0, int y1)
Rasterizes a line in the costmap grid and checks for collisions.
sensor_msgs::PointCloud cloud
double max_z_
The height cutoff for adding points as obstacles.
void insert(pcl::PointXYZ pt)
bool worldToMap2D(double wx, double wy, unsigned int &mx, unsigned int &my)
TFSIMD_FORCE_INLINE const tfScalar & x() const
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)
Checks if any obstacles in the voxel grid lie inside a convex footprint that is rasterized into the g...
TFSIMD_FORCE_INLINE const tfScalar & z() const
void getPoints(pcl::PointCloud< pcl::PointXYZ > &cloud)
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
voxel_grid::VoxelGrid obstacle_grid_
void removePointsInScanBoundry(const PlanarLaserScan &laser_scan, double raytrace_range)
double dist(double x0, double y0, double z0, double x1, double y1, double z1)
Stores a scan from a planar laser that can be used to clear freespace.
void clearVoxelLine(double x0, double y0, double z0, double x1, double y1, double z1, unsigned int max_length=UINT_MAX)