voxel_grid_model.h
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2008, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the Willow Garage nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Eitan Marder-Eppstein
36 *********************************************************************/
37 #ifndef TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
38 #define TRAJECTORY_ROLLOUT_VOXEL_WORLD_MODEL_H_
39 #include <vector>
40 #include <list>
41 #include <cfloat>
42 #include <geometry_msgs/Point.h>
43 #include <costmap_2d/observation.h>
45 
46 #include <pcl/point_types.h>
47 #include <pcl/point_cloud.h>
48 
49 //voxel grid stuff
50 #include <voxel_grid/voxel_grid.h>
51 
52 namespace base_local_planner {
58  class VoxelGridModel : public WorldModel {
59  public:
73  VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution,
74  double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range);
75 
79  virtual ~VoxelGridModel(){}
80 
89  virtual double footprintCost(const geometry_msgs::Point& position, const std::vector<geometry_msgs::Point>& footprint,
90  double inscribed_radius, double circumscribed_radius);
91 
93 
100  void updateWorld(const std::vector<geometry_msgs::Point>& footprint,
101  const std::vector<costmap_2d::Observation>& observations, const std::vector<PlanarLaserScan>& laser_scans);
102 
103  void getPoints(pcl::PointCloud<pcl::PointXYZ>& cloud);
104 
105  private:
114  double lineCost(int x0, int x1, int y0, int y1);
115 
122  double pointCost(int x, int y);
123 
124  void removePointsInScanBoundry(const PlanarLaserScan& laser_scan, double raytrace_range);
125 
126  inline bool worldToMap3D(double wx, double wy, double wz, unsigned int& mx, unsigned int& my, unsigned int& mz){
127  if(wx < origin_x_ || wy < origin_y_ || wz < origin_z_)
128  return false;
129  mx = (int) ((wx - origin_x_) / xy_resolution_);
130  my = (int) ((wy - origin_y_) / xy_resolution_);
131  mz = (int) ((wz - origin_z_) / z_resolution_);
132  return true;
133  }
134 
135  inline bool worldToMap2D(double wx, double wy, unsigned int& mx, unsigned int& my){
136  if(wx < origin_x_ || wy < origin_y_)
137  return false;
138  mx = (int) ((wx - origin_x_) / xy_resolution_);
139  my = (int) ((wy - origin_y_) / xy_resolution_);
140  return true;
141  }
142 
143  inline void mapToWorld3D(unsigned int mx, unsigned int my, unsigned int mz, double& wx, double& wy, double& wz){
144  //returns the center point of the cell
145  wx = origin_x_ + (mx + 0.5) * xy_resolution_;
146  wy = origin_y_ + (my + 0.5) * xy_resolution_;
147  wz = origin_z_ + (mz + 0.5) * z_resolution_;
148  }
149 
150  inline void mapToWorld2D(unsigned int mx, unsigned int my, double& wx, double& wy){
151  //returns the center point of the cell
152  wx = origin_x_ + (mx + 0.5) * xy_resolution_;
153  wy = origin_y_ + (my + 0.5) * xy_resolution_;
154  }
155 
156  inline double dist(double x0, double y0, double z0, double x1, double y1, double z1){
157  return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
158  }
159 
160  inline void insert(pcl::PointXYZ pt){
161  unsigned int cell_x, cell_y, cell_z;
162  if(!worldToMap3D(pt.x, pt.y, pt.z, cell_x, cell_y, cell_z))
163  return;
164  obstacle_grid_.markVoxel(cell_x, cell_y, cell_z);
165  }
166 
170  double origin_x_;
171  double origin_y_;
172  double origin_z_;
173  double max_z_;
175 
176  };
177 };
178 #endif
double sq_obstacle_range_
The square distance at which we no longer add obstacles to the grid.
double pointCost(int x, int y)
Checks the cost of a point in the costmap.
virtual double footprintCost(const geometry_msgs::Point &position, const std::vector< geometry_msgs::Point > &footprint, double inscribed_radius, double circumscribed_radius)=0
Subclass will implement this method to check a footprint at a given position and orientation for lega...
void mapToWorld2D(unsigned int mx, unsigned int my, double &wx, double &wy)
VoxelGridModel(double size_x, double size_y, double size_z, double xy_resolution, double z_resolution, double origin_x, double origin_y, double origin_z, double max_z, double obstacle_range)
Constructor for the VoxelGridModel.
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
bool worldToMap3D(double wx, double wy, double wz, unsigned int &mx, unsigned int &my, unsigned int &mz)
TFSIMD_FORCE_INLINE const tfScalar & y() const
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...
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.
double max_z_
The height cutoff for adding points as obstacles.
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...
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
virtual ~VoxelGridModel()
Destructor for the world model.
void markVoxel(unsigned int x, unsigned int y, unsigned int z)
void getPoints(pcl::PointCloud< pcl::PointXYZ > &cloud)
An interface the trajectory controller uses to interact with the world regardless of the underlying w...
Definition: world_model.h:52
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.


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49