distance_field.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */
36 
41 #include <console_bridge/console.h>
42 #include <octomap/octomap.h>
43 #include <octomap/OcTree.h>
44 
45 distance_field::DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution,
46  double origin_x, double origin_y, double origin_z)
47  : size_x_(size_x)
48  , size_y_(size_y)
49  , size_z_(size_z)
50  , origin_x_(origin_x)
51  , origin_y_(origin_y)
52  , origin_z_(origin_z)
53  , resolution_(resolution)
54  , inv_twice_resolution_(1.0 / (2.0 * resolution_))
55 {
56 }
57 
59 {
60 }
61 
62 double distance_field::DistanceField::getDistanceGradient(double x, double y, double z, double& gradient_x,
63  double& gradient_y, double& gradient_z, bool& in_bounds) const
64 {
65  int gx, gy, gz;
66 
67  worldToGrid(x, y, z, gx, gy, gz);
68 
69  // if out of bounds, return max_distance, and 0 gradient
70  // we need extra padding of 1 to get gradients
71  if (gx < 1 || gy < 1 || gz < 1 || gx >= getXNumCells() - 1 || gy >= getYNumCells() - 1 || gz >= getZNumCells() - 1)
72  {
73  gradient_x = 0.0;
74  gradient_y = 0.0;
75  gradient_z = 0.0;
76  in_bounds = false;
77  return getUninitializedDistance();
78  }
79 
80  gradient_x = (getDistance(gx + 1, gy, gz) - getDistance(gx - 1, gy, gz)) * inv_twice_resolution_;
81  gradient_y = (getDistance(gx, gy + 1, gz) - getDistance(gx, gy - 1, gz)) * inv_twice_resolution_;
82  gradient_z = (getDistance(gx, gy, gz + 1) - getDistance(gx, gy, gz - 1)) * inv_twice_resolution_;
83 
84  in_bounds = true;
85  return getDistance(gx, gy, gz);
86 }
87 
88 void distance_field::DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance,
89  const std::string& frame_id, const ros::Time stamp,
90  visualization_msgs::Marker& inf_marker) const
91 {
92  inf_marker.points.clear();
93  inf_marker.header.frame_id = frame_id;
94  inf_marker.header.stamp = stamp;
95  inf_marker.ns = "distance_field";
96  inf_marker.id = 1;
97  inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
98  inf_marker.action = visualization_msgs::Marker::MODIFY;
99  inf_marker.scale.x = resolution_;
100  inf_marker.scale.y = resolution_;
101  inf_marker.scale.z = resolution_;
102  inf_marker.color.r = 1.0;
103  inf_marker.color.g = 0.0;
104  inf_marker.color.b = 0.0;
105  inf_marker.color.a = 0.1;
106  // inf_marker.lifetime = ros::Duration(30.0);
107 
108  inf_marker.points.reserve(100000);
109  for (int x = 0; x < getXNumCells(); ++x)
110  {
111  for (int y = 0; y < getYNumCells(); ++y)
112  {
113  for (int z = 0; z < getZNumCells(); ++z)
114  {
115  double dist = getDistance(x, y, z);
116 
117  if (dist >= min_distance && dist <= max_distance)
118  {
119  int last = inf_marker.points.size();
120  inf_marker.points.resize(last + 1);
121  double nx, ny, nz;
122  gridToWorld(x, y, z, nx, ny, nz);
123  Eigen::Translation3d vec(nx, ny, nz);
124  inf_marker.points[last].x = vec.x();
125  inf_marker.points[last].y = vec.y();
126  inf_marker.points[last].z = vec.z();
127  }
128  }
129  }
130  }
131 }
132 
133 void distance_field::DistanceField::getGradientMarkers(double min_distance, double max_distance,
134  const std::string& frame_id, const ros::Time& stamp,
135  visualization_msgs::MarkerArray& marker_array) const
136 {
137  Eigen::Vector3d unitX(1, 0, 0);
138  Eigen::Vector3d unitY(0, 1, 0);
139  Eigen::Vector3d unitZ(0, 0, 1);
140 
141  int id = 0;
142 
143  for (int x = 0; x < getXNumCells(); ++x)
144  {
145  for (int y = 0; y < getYNumCells(); ++y)
146  {
147  for (int z = 0; z < getZNumCells(); ++z)
148  {
149  double worldX, worldY, worldZ;
150  gridToWorld(x, y, z, worldX, worldY, worldZ);
151 
152  double gradientX, gradientY, gradientZ;
153  bool in_bounds;
154  double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ, in_bounds);
155  Eigen::Vector3d gradient(gradientX, gradientY, gradientZ);
156 
157  if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
158  {
159  visualization_msgs::Marker marker;
160 
161  marker.header.frame_id = frame_id;
162  marker.header.stamp = stamp;
163 
164  marker.ns = "distance_field_gradient";
165  marker.id = id++;
166  marker.type = visualization_msgs::Marker::ARROW;
167  marker.action = visualization_msgs::Marker::ADD;
168 
169  marker.pose.position.x = worldX;
170  marker.pose.position.y = worldY;
171  marker.pose.position.z = worldZ;
172 
173  // Eigen::Vector3d axis = gradient.cross(unitX).norm() > 0 ? gradient.cross(unitX) : unitY;
174  // double angle = -gradient.angle(unitX);
175  // Eigen::AngleAxisd rotation(angle, axis);
176 
177  // marker.pose.orientation.x = rotation.rotation().x();
178  // marker.pose.orientation.y = rotation.rotation().y();
179  // marker.pose.orientation.z = rotation.rotation().z();
180  // marker.pose.orientation.w = rotation.rotation().w();
181 
182  marker.scale.x = getResolution();
183  marker.scale.y = getResolution();
184  marker.scale.z = getResolution();
185 
186  marker.color.r = 0.0;
187  marker.color.g = 0.0;
188  marker.color.b = 1.0;
189  marker.color.a = 1.0;
190 
191  marker_array.markers.push_back(marker);
192  }
193  }
194  }
195  }
196 }
197 
198 bool distance_field::DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Affine3d& pose,
200 {
201  if (shape->type == shapes::OCTREE)
202  {
203  const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
204  if (!oc)
205  {
206  logError("Problem dynamic casting shape that claims to be OcTree");
207  return false;
208  }
209  getOcTreePoints(oc->octree.get(), points);
210  }
211  else
212  {
214  body->setPose(pose);
215  findInternalPointsConvex(*body, resolution_, *points);
216  delete body;
217  }
218  return true;
219 }
220 
221 void distance_field::DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Affine3d& pose)
222 {
223  EigenSTL::vector_Vector3d point_vec;
224  getShapePoints(shape, pose, &point_vec);
225  addPointsToField(point_vec);
226 }
227 
228 // DEPRECATED
229 void distance_field::DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
230 {
231  Eigen::Affine3d pose_e;
232  tf::poseMsgToEigen(pose, pose_e);
233  addShapeToField(shape, pose_e);
234 }
235 
237 {
238  // lower extent
239  double min_x, min_y, min_z;
240  gridToWorld(0, 0, 0, min_x, min_y, min_z);
241 
242  octomap::point3d bbx_min(min_x, min_y, min_z);
243 
244  int num_x = getXNumCells();
245  int num_y = getYNumCells();
246  int num_z = getZNumCells();
247 
248  // upper extent
249  double max_x, max_y, max_z;
250  gridToWorld(num_x, num_y, num_z, max_x, max_y, max_z);
251 
252  octomap::point3d bbx_max(max_x, max_y, max_z);
253 
254  for (octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min, bbx_max), end = octree->end_leafs_bbx();
255  it != end; ++it)
256  {
257  if (octree->isNodeOccupied(*it))
258  {
259  if (it.getSize() <= resolution_)
260  {
261  Eigen::Vector3d point(it.getX(), it.getY(), it.getZ());
262  points->push_back(point);
263  }
264  else
265  {
266  double ceil_val = ceil(it.getSize() / resolution_) * resolution_ / 2.0;
267  for (double x = it.getX() - ceil_val; x <= it.getX() + ceil_val; x += resolution_)
268  {
269  for (double y = it.getY() - ceil_val; y <= it.getY() + ceil_val; y += resolution_)
270  {
271  for (double z = it.getZ() - ceil_val; z <= it.getZ() + ceil_val; z += resolution_)
272  {
273  points->push_back(Eigen::Vector3d(x, y, z));
274  }
275  }
276  }
277  }
278  }
279  }
280 }
281 
283 {
285  getOcTreePoints(octree, &points);
286  addPointsToField(points);
287 }
288 
289 void distance_field::DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Affine3d& old_pose,
290  const Eigen::Affine3d& new_pose)
291 {
292  if (shape->type == shapes::OCTREE)
293  {
294  logWarn("Move shape not supported for Octree");
295  return;
296  }
298  body->setPose(old_pose);
299  EigenSTL::vector_Vector3d old_point_vec;
300  findInternalPointsConvex(*body, resolution_, old_point_vec);
301  body->setPose(new_pose);
302  EigenSTL::vector_Vector3d new_point_vec;
303  findInternalPointsConvex(*body, resolution_, new_point_vec);
304  delete body;
305  updatePointsInField(old_point_vec, new_point_vec);
306 }
307 
308 // DEPRECATED
309 void distance_field::DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
310  const geometry_msgs::Pose& new_pose)
311 {
312  Eigen::Affine3d old_pose_e, new_pose_e;
313  tf::poseMsgToEigen(old_pose, old_pose_e);
314  tf::poseMsgToEigen(new_pose, new_pose_e);
315  moveShapeInField(shape, old_pose_e, new_pose_e);
316 }
317 
318 void distance_field::DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Affine3d& pose)
319 {
321  body->setPose(pose);
322  EigenSTL::vector_Vector3d point_vec;
323  findInternalPointsConvex(*body, resolution_, point_vec);
324  delete body;
325  removePointsFromField(point_vec);
326 }
327 
328 // DEPRECATED
329 void distance_field::DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
330 {
331  Eigen::Affine3d pose_e;
332  tf::poseMsgToEigen(pose, pose_e);
333  removeShapeFromField(shape, pose_e);
334 }
335 
337  double width, double height, const Eigen::Vector3d& origin,
338  const std::string& frame_id, const ros::Time stamp,
339  visualization_msgs::Marker& plane_marker) const
340 {
341  plane_marker.header.frame_id = frame_id;
342  plane_marker.header.stamp = stamp;
343  plane_marker.ns = "distance_field_plane";
344  plane_marker.id = 1;
345  plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
346  plane_marker.action = visualization_msgs::Marker::ADD;
347  plane_marker.scale.x = resolution_;
348  plane_marker.scale.y = resolution_;
349  plane_marker.scale.z = resolution_;
350  // plane_marker.lifetime = ros::Duration(30.0);
351 
352  plane_marker.points.reserve(100000);
353  plane_marker.colors.reserve(100000);
354 
355  double minX = 0;
356  double maxX = 0;
357  double minY = 0;
358  double maxY = 0;
359  double minZ = 0;
360  double maxZ = 0;
361 
362  switch (type)
363  {
364  case XYPlane:
365  minZ = height;
366  maxZ = height;
367 
368  minX = -length / 2.0;
369  maxX = length / 2.0;
370  minY = -width / 2.0;
371  maxY = width / 2.0;
372  break;
373  case XZPlane:
374  minY = height;
375  maxY = height;
376 
377  minX = -length / 2.0;
378  maxX = length / 2.0;
379  minZ = -width / 2.0;
380  maxZ = width / 2.0;
381  break;
382  case YZPlane:
383  minX = height;
384  maxX = height;
385 
386  minY = -length / 2.0;
387  maxY = length / 2.0;
388  minZ = -width / 2.0;
389  maxZ = width / 2.0;
390  break;
391  }
392 
393  minX += origin.x();
394  minY += origin.y();
395  minZ += origin.z();
396 
397  maxX += origin.x();
398  maxY += origin.y();
399  maxZ += origin.z();
400 
401  int minXCell = 0;
402  int maxXCell = 0;
403  int minYCell = 0;
404  int maxYCell = 0;
405  int minZCell = 0;
406  int maxZCell = 0;
407 
408  worldToGrid(minX, minY, minZ, minXCell, minYCell, minZCell);
409  worldToGrid(maxX, maxY, maxZ, maxXCell, maxYCell, maxZCell);
410  plane_marker.color.a = 1.0;
411  for (int x = minXCell; x <= maxXCell; ++x)
412  {
413  for (int y = minYCell; y <= maxYCell; ++y)
414  {
415  for (int z = minZCell; z <= maxZCell; ++z)
416  {
417  if (!isCellValid(x, y, z))
418  {
419  continue;
420  }
421  double dist = getDistance(x, y, z);
422  int last = plane_marker.points.size();
423  plane_marker.points.resize(last + 1);
424  plane_marker.colors.resize(last + 1);
425  double nx, ny, nz;
426  gridToWorld(x, y, z, nx, ny, nz);
427  Eigen::Vector3d vec(nx, ny, nz);
428  plane_marker.points[last].x = vec.x();
429  plane_marker.points[last].y = vec.y();
430  plane_marker.points[last].z = vec.z();
431  if (dist < 0.0)
432  {
433  plane_marker.colors[last].r = fmax(fmin(0.1 / fabs(dist), 1.0), 0.0);
434  plane_marker.colors[last].g = fmax(fmin(0.05 / fabs(dist), 1.0), 0.0);
435  plane_marker.colors[last].b = fmax(fmin(0.01 / fabs(dist), 1.0), 0.0);
436  }
437  else
438  {
439  plane_marker.colors[last].b = fmax(fmin(0.1 / (dist + 0.001), 1.0), 0.0);
440  plane_marker.colors[last].g = fmax(fmin(0.05 / (dist + 0.001), 1.0), 0.0);
441  plane_marker.colors[last].r = fmax(fmin(0.01 / (dist + 0.001), 1.0), 0.0);
442  }
443  }
444  }
445  }
446 }
447 
448 void distance_field::DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point,
449  std_msgs::ColorRGBA& color, double max_distance) const
450 {
451  double wx, wy, wz;
452  gridToWorld(xCell, yCell, zCell, wx, wy, wz);
453 
454  point.x = wx;
455  point.y = wy;
456  point.z = wz;
457 
458  color.r = 1.0;
459  color.g = dist / max_distance; // dist/max_distance * 0.5;
460  color.b = dist / max_distance; // dist/max_distance * 0.1;
461 }
462 
463 void distance_field::DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp,
464  double max_dist, visualization_msgs::Marker& marker) const
465 {
466  int maxXCell = getXNumCells();
467  int maxYCell = getYNumCells();
468  int maxZCell = getZNumCells();
469 
470  double* x_projection = new double[maxYCell * maxZCell];
471  double* y_projection = new double[maxZCell * maxXCell];
472  double* z_projection = new double[maxXCell * maxYCell];
473  double initial_val = sqrt(INT_MAX);
474 
475  // Initialize
476  for (int y = 0; y < maxYCell; y++)
477  for (int x = 0; x < maxXCell; x++)
478  z_projection[x + y * maxXCell] = initial_val;
479 
480  for (int z = 0; z < maxZCell; z++)
481  for (int y = 0; y < maxYCell; y++)
482  x_projection[y + z * maxYCell] = initial_val;
483 
484  for (int z = 0; z < maxZCell; z++)
485  for (int x = 0; x < maxXCell; x++)
486  y_projection[x + z * maxXCell] = initial_val;
487 
488  // Calculate projections
489  for (int z = 0; z < maxZCell; z++)
490  {
491  for (int y = 0; y < maxYCell; y++)
492  {
493  for (int x = 0; x < maxXCell; x++)
494  {
495  double dist = getDistance(x, y, z);
496  z_projection[x + y * maxXCell] = std::min(dist, z_projection[x + y * maxXCell]);
497  x_projection[y + z * maxYCell] = std::min(dist, x_projection[y + z * maxYCell]);
498  y_projection[x + z * maxXCell] = std::min(dist, y_projection[x + z * maxXCell]);
499  }
500  }
501  }
502 
503  // Make markers
504  marker.points.clear();
505  marker.header.frame_id = frame_id;
506  marker.header.stamp = stamp;
507  marker.ns = "distance_field_projection_plane";
508  marker.id = 1;
509  marker.type = visualization_msgs::Marker::CUBE_LIST;
510  marker.action = visualization_msgs::Marker::MODIFY;
511  marker.scale.x = getResolution();
512  marker.scale.y = getResolution();
513  marker.scale.z = getResolution();
514  marker.color.a = 1.0;
515  // marker.lifetime = ros::Duration(30.0);
516 
517  int x, y, z;
518  int index = 0;
519  marker.points.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
520  marker.colors.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
521 
522  z = 0;
523  for (y = 0; y < maxYCell; y++)
524  {
525  for (x = 0; x < maxXCell; x++)
526  {
527  double dist = z_projection[x + y * maxXCell];
528  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
529  index++;
530  }
531  }
532 
533  x = 0;
534  for (z = 0; z < maxZCell; z++)
535  {
536  for (y = 0; y < maxYCell; y++)
537  {
538  double dist = x_projection[y + z * maxYCell];
539  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
540  index++;
541  }
542  }
543 
544  y = 0;
545  for (z = 0; z < maxZCell; z++)
546  {
547  for (x = 0; x < maxXCell; x++)
548  {
549  double dist = y_projection[x + z * maxXCell];
550  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
551  index++;
552  }
553  }
554 
555  if (x_projection)
556  delete[] x_projection;
557  if (y_projection)
558  delete[] y_projection;
559  if (z_projection)
560  delete[] z_projection;
561 }
virtual int getYNumCells() const =0
Gets the number of cells along the Y axis.
void setPose(const Eigen::Affine3d &pose)
virtual double getDistance(double x, double y, double z) const =0
Gets the distance to the closest obstacle at the given location. The particulars of this function are...
PlaneVisualizationType
The plane to visualize.
void getOcTreePoints(const octomap::OcTree *octree, EigenSTL::vector_Vector3d *points)
Get the points associated with an octree.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
static const double width
virtual bool gridToWorld(int x, int y, int z, double &world_x, double &world_y, double &world_z) const =0
Converts from an set of integer indices to a world location given the origin and resolution parameter...
virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)=0
Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells.
void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point &point, std_msgs::ColorRGBA &color, double max_distance) const
Helper function that sets the point value and color given the distance.
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ShapeType type
virtual int getZNumCells() const =0
Gets the number of cells along the Z axis.
static const double origin_y
leaf_bbx_iterator begin_leafs_bbx(const OcTreeKey &min, const OcTreeKey &max, unsigned char maxDepth=0) const
void removeShapeFromField(const shapes::Shape *shape, const Eigen::Affine3d &pose)
All points corresponding to the shape are removed from the distance field.
double getResolution() const
Gets the resolution of the distance field in meters.
DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z)
Constructor, where units are arbitrary but are assumed to be meters.
double getDistanceGradient(double x, double y, double z, double &gradient_x, double &gradient_y, double &gradient_z, bool &in_bounds) const
Gets not only the distance to the nearest cell but the gradient direction. The gradient is computed a...
bool getShapePoints(const shapes::Shape *shape, const Eigen::Affine3d &pose, EigenSTL::vector_Vector3d *points)
Get the points associated with a shape. This is mainly used when the external application needs to ca...
double y
std::shared_ptr< const octomap::OcTree > octree
unsigned int index
virtual void updatePointsInField(const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points)=0
This function will remove any obstacle points that are in the old point set but not the new point set...
static const double resolution
virtual bool isCellValid(int x, int y, int z) const =0
Determines whether or not the cell associated with the supplied indices is valid for this distance fi...
static const double origin_z
void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string &frame_id, const ros::Time stamp, visualization_msgs::Marker &marker) const
Get an iso-surface for visualization in rviz. The iso-surface shows every cell that has a distance in...
Body * createBodyFromShape(const shapes::Shape *shape)
virtual double getUninitializedDistance() const =0
Gets a distance value for an invalid cell.
double z
void getProjectionPlanes(const std::string &frame_id, const ros::Time &stamp, double max_distance, visualization_msgs::Marker &marker) const
A function that populates the marker with three planes - one each along the XY, XZ, and YZ axes. For each of the planes, any column on that plane will be marked according to the minimum distance along that column.
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
virtual int getXNumCells() const =0
Gets the number of cells along the X axis.
static const double height
virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)=0
Remove a set of obstacle points from the distance field, updating distance values accordingly...
virtual bool worldToGrid(double world_x, double world_y, double world_z, int &x, int &y, int &z) const =0
Converts from a world location to a set of integer indices. Should return false if the world location...
void findInternalPointsConvex(const bodies::Body &body, double resolution, EigenSTL::vector_Vector3d &points)
Find all points on a regular grid that are internal to the body, assuming the body is a convex shape...
void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height, const Eigen::Vector3d &origin, const std::string &frame_id, const ros::Time stamp, visualization_msgs::Marker &marker) const
Populates a marker with a slice of the distance field in a particular plane. All cells in the plane w...
static const double max_dist
void addOcTreeToField(const octomap::OcTree *octree)
Adds an octree to the distance field. Cells that are occupied in the octree that lie within the voxel...
void getGradientMarkers(double min_radius, double max_radius, const std::string &frame_id, const ros::Time &stamp, visualization_msgs::MarkerArray &marker_array) const
Populates the supplied marker array with a series of arrows representing gradients of cells that are ...
double resolution_
Resolution of the distance field.
double x
void addShapeToField(const shapes::Shape *shape, const Eigen::Affine3d &pose)
Adds the set of points corresponding to the shape at the given pose as obstacle points into the dista...
bool isNodeOccupied(const OcTreeNode *occupancyNode) const
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:55
int inv_twice_resolution_
Computed value 1.0/(2.0*resolution_)
void moveShapeInField(const shapes::Shape *shape, const Eigen::Affine3d &old_pose, const Eigen::Affine3d &new_pose)
Moves the shape in the distance field from the old pose to the new pose, removing points that are no ...
static const double origin_x


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Mon Jan 15 2018 03:50:44