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 <ros/console.h>
42 #include <octomap/octomap.h>
43 #include <octomap/OcTree.h>
44 
45 namespace distance_field
46 {
47 DistanceField::DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x,
48  double origin_y, double origin_z)
49  : size_x_(size_x)
50  , size_y_(size_y)
51  , size_z_(size_z)
52  , origin_x_(origin_x)
53  , origin_y_(origin_y)
54  , origin_z_(origin_z)
55  , resolution_(resolution)
56  , inv_twice_resolution_(1.0 / (2.0 * resolution_))
57 {
58 }
59 
61 
62 double DistanceField::getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y,
63  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 DistanceField::getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
89  const ros::Time stamp, visualization_msgs::Marker& inf_marker) const
90 {
91  inf_marker.points.clear();
92  inf_marker.header.frame_id = frame_id;
93  inf_marker.header.stamp = stamp;
94  inf_marker.ns = "distance_field";
95  inf_marker.id = 1;
96  inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
97  inf_marker.action = visualization_msgs::Marker::MODIFY;
98  inf_marker.scale.x = resolution_;
99  inf_marker.scale.y = resolution_;
100  inf_marker.scale.z = resolution_;
101  inf_marker.color.r = 1.0;
102  inf_marker.color.g = 0.0;
103  inf_marker.color.b = 0.0;
104  inf_marker.color.a = 0.1;
105  // inf_marker.lifetime = ros::Duration(30.0);
106 
107  inf_marker.points.reserve(100000);
108  for (int x = 0; x < getXNumCells(); ++x)
109  {
110  for (int y = 0; y < getYNumCells(); ++y)
111  {
112  for (int z = 0; z < getZNumCells(); ++z)
113  {
114  double dist = getDistance(x, y, z);
115 
116  if (dist >= min_distance && dist <= max_distance)
117  {
118  int last = inf_marker.points.size();
119  inf_marker.points.resize(last + 1);
120  double nx, ny, nz;
121  gridToWorld(x, y, z, nx, ny, nz);
122  Eigen::Translation3d vec(nx, ny, nz);
123  inf_marker.points[last].x = vec.x();
124  inf_marker.points[last].y = vec.y();
125  inf_marker.points[last].z = vec.z();
126  }
127  }
128  }
129  }
130 }
131 
132 void DistanceField::getGradientMarkers(double min_distance, double max_distance, const std::string& frame_id,
133  const ros::Time& stamp, visualization_msgs::MarkerArray& marker_array) const
134 {
135  Eigen::Vector3d unitX(1, 0, 0);
136  Eigen::Vector3d unitY(0, 1, 0);
137  Eigen::Vector3d unitZ(0, 0, 1);
138 
139  int id = 0;
140 
141  for (int x = 0; x < getXNumCells(); ++x)
142  {
143  for (int y = 0; y < getYNumCells(); ++y)
144  {
145  for (int z = 0; z < getZNumCells(); ++z)
146  {
147  double worldX, worldY, worldZ;
148  gridToWorld(x, y, z, worldX, worldY, worldZ);
149 
150  double gradientX, gradientY, gradientZ;
151  bool in_bounds;
152  double distance = getDistanceGradient(worldX, worldY, worldZ, gradientX, gradientY, gradientZ, in_bounds);
153  Eigen::Vector3d gradient(gradientX, gradientY, gradientZ);
154 
155  if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
156  {
157  visualization_msgs::Marker marker;
158 
159  marker.header.frame_id = frame_id;
160  marker.header.stamp = stamp;
161 
162  marker.ns = "distance_field_gradient";
163  marker.id = id++;
164  marker.type = visualization_msgs::Marker::ARROW;
165  marker.action = visualization_msgs::Marker::ADD;
166 
167  marker.pose.position.x = worldX;
168  marker.pose.position.y = worldY;
169  marker.pose.position.z = worldZ;
170 
171  // Eigen::Vector3d axis = gradient.cross(unitX).norm() > 0 ? gradient.cross(unitX) : unitY;
172  // double angle = -gradient.angle(unitX);
173  // Eigen::AngleAxisd rotation(angle, axis);
174 
175  // marker.pose.orientation.x = rotation.linear().x();
176  // marker.pose.orientation.y = rotation.linear().y();
177  // marker.pose.orientation.z = rotation.linear().z();
178  // marker.pose.orientation.w = rotation.linear().w();
179 
180  marker.scale.x = getResolution();
181  marker.scale.y = getResolution();
182  marker.scale.z = getResolution();
183 
184  marker.color.r = 0.0;
185  marker.color.g = 0.0;
186  marker.color.b = 1.0;
187  marker.color.a = 1.0;
188 
189  marker_array.markers.push_back(marker);
190  }
191  }
192  }
193  }
194 }
195 
196 bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Affine3d& pose,
198 {
199  if (shape->type == shapes::OCTREE)
200  {
201  const shapes::OcTree* oc = dynamic_cast<const shapes::OcTree*>(shape);
202  if (!oc)
203  {
204  ROS_ERROR_NAMED("distance_field", "Problem dynamic casting shape that claims to be OcTree");
205  return false;
206  }
207  getOcTreePoints(oc->octree.get(), points);
208  }
209  else
210  {
212  body->setPose(pose);
213  findInternalPointsConvex(*body, resolution_, *points);
214  delete body;
215  }
216  return true;
217 }
218 
219 void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Affine3d& pose)
220 {
221  EigenSTL::vector_Vector3d point_vec;
222  getShapePoints(shape, pose, &point_vec);
223  addPointsToField(point_vec);
224 }
225 
226 // DEPRECATED
227 void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
228 {
229  Eigen::Affine3d pose_e;
230  tf::poseMsgToEigen(pose, pose_e);
231  addShapeToField(shape, pose_e);
232 }
233 
235 {
236  // lower extent
237  double min_x, min_y, min_z;
238  gridToWorld(0, 0, 0, min_x, min_y, min_z);
239 
240  octomap::point3d bbx_min(min_x, min_y, min_z);
241 
242  int num_x = getXNumCells();
243  int num_y = getYNumCells();
244  int num_z = getZNumCells();
245 
246  // upper extent
247  double max_x, max_y, max_z;
248  gridToWorld(num_x, num_y, num_z, max_x, max_y, max_z);
249 
250  octomap::point3d bbx_max(max_x, max_y, max_z);
251 
252  for (octomap::OcTree::leaf_bbx_iterator it = octree->begin_leafs_bbx(bbx_min, bbx_max), end = octree->end_leafs_bbx();
253  it != end; ++it)
254  {
255  if (octree->isNodeOccupied(*it))
256  {
257  if (it.getSize() <= resolution_)
258  {
259  Eigen::Vector3d point(it.getX(), it.getY(), it.getZ());
260  points->push_back(point);
261  }
262  else
263  {
264  double ceil_val = ceil(it.getSize() / resolution_) * resolution_ / 2.0;
265  for (double x = it.getX() - ceil_val; x <= it.getX() + ceil_val; x += resolution_)
266  {
267  for (double y = it.getY() - ceil_val; y <= it.getY() + ceil_val; y += resolution_)
268  {
269  for (double z = it.getZ() - ceil_val; z <= it.getZ() + ceil_val; z += resolution_)
270  {
271  points->push_back(Eigen::Vector3d(x, y, z));
272  }
273  }
274  }
275  }
276  }
277  }
278 }
279 
281 {
283  getOcTreePoints(octree, &points);
284  addPointsToField(points);
285 }
286 
287 void DistanceField::moveShapeInField(const shapes::Shape* shape, const Eigen::Affine3d& old_pose,
288  const Eigen::Affine3d& new_pose)
289 {
290  if (shape->type == shapes::OCTREE)
291  {
292  ROS_WARN_NAMED("distance_field", "Move shape not supported for Octree");
293  return;
294  }
296  body->setPose(old_pose);
297  EigenSTL::vector_Vector3d old_point_vec;
298  findInternalPointsConvex(*body, resolution_, old_point_vec);
299  body->setPose(new_pose);
300  EigenSTL::vector_Vector3d new_point_vec;
301  findInternalPointsConvex(*body, resolution_, new_point_vec);
302  delete body;
303  updatePointsInField(old_point_vec, new_point_vec);
304 }
305 
306 // DEPRECATED
307 void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
308  const geometry_msgs::Pose& new_pose)
309 {
310  Eigen::Affine3d old_pose_e, new_pose_e;
311  tf::poseMsgToEigen(old_pose, old_pose_e);
312  tf::poseMsgToEigen(new_pose, new_pose_e);
313  moveShapeInField(shape, old_pose_e, new_pose_e);
314 }
315 
316 void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen::Affine3d& pose)
317 {
319  body->setPose(pose);
320  EigenSTL::vector_Vector3d point_vec;
321  findInternalPointsConvex(*body, resolution_, point_vec);
322  delete body;
323  removePointsFromField(point_vec);
324 }
325 
326 // DEPRECATED
327 void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
328 {
329  Eigen::Affine3d pose_e;
330  tf::poseMsgToEigen(pose, pose_e);
331  removeShapeFromField(shape, pose_e);
332 }
333 
334 void DistanceField::getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
335  const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp,
336  visualization_msgs::Marker& plane_marker) const
337 {
338  plane_marker.header.frame_id = frame_id;
339  plane_marker.header.stamp = stamp;
340  plane_marker.ns = "distance_field_plane";
341  plane_marker.id = 1;
342  plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
343  plane_marker.action = visualization_msgs::Marker::ADD;
344  plane_marker.scale.x = resolution_;
345  plane_marker.scale.y = resolution_;
346  plane_marker.scale.z = resolution_;
347  // plane_marker.lifetime = ros::Duration(30.0);
348 
349  plane_marker.points.reserve(100000);
350  plane_marker.colors.reserve(100000);
351 
352  double minX = 0;
353  double maxX = 0;
354  double minY = 0;
355  double maxY = 0;
356  double minZ = 0;
357  double maxZ = 0;
358 
359  switch (type)
360  {
361  case XYPlane:
362  minZ = height;
363  maxZ = height;
364 
365  minX = -length / 2.0;
366  maxX = length / 2.0;
367  minY = -width / 2.0;
368  maxY = width / 2.0;
369  break;
370  case XZPlane:
371  minY = height;
372  maxY = height;
373 
374  minX = -length / 2.0;
375  maxX = length / 2.0;
376  minZ = -width / 2.0;
377  maxZ = width / 2.0;
378  break;
379  case YZPlane:
380  minX = height;
381  maxX = height;
382 
383  minY = -length / 2.0;
384  maxY = length / 2.0;
385  minZ = -width / 2.0;
386  maxZ = width / 2.0;
387  break;
388  }
389 
390  minX += origin.x();
391  minY += origin.y();
392  minZ += origin.z();
393 
394  maxX += origin.x();
395  maxY += origin.y();
396  maxZ += origin.z();
397 
398  int minXCell = 0;
399  int maxXCell = 0;
400  int minYCell = 0;
401  int maxYCell = 0;
402  int minZCell = 0;
403  int maxZCell = 0;
404 
405  worldToGrid(minX, minY, minZ, minXCell, minYCell, minZCell);
406  worldToGrid(maxX, maxY, maxZ, maxXCell, maxYCell, maxZCell);
407  plane_marker.color.a = 1.0;
408  for (int x = minXCell; x <= maxXCell; ++x)
409  {
410  for (int y = minYCell; y <= maxYCell; ++y)
411  {
412  for (int z = minZCell; z <= maxZCell; ++z)
413  {
414  if (!isCellValid(x, y, z))
415  {
416  continue;
417  }
418  double dist = getDistance(x, y, z);
419  int last = plane_marker.points.size();
420  plane_marker.points.resize(last + 1);
421  plane_marker.colors.resize(last + 1);
422  double nx, ny, nz;
423  gridToWorld(x, y, z, nx, ny, nz);
424  Eigen::Vector3d vec(nx, ny, nz);
425  plane_marker.points[last].x = vec.x();
426  plane_marker.points[last].y = vec.y();
427  plane_marker.points[last].z = vec.z();
428  if (dist < 0.0)
429  {
430  plane_marker.colors[last].r = fmax(fmin(0.1 / fabs(dist), 1.0), 0.0);
431  plane_marker.colors[last].g = fmax(fmin(0.05 / fabs(dist), 1.0), 0.0);
432  plane_marker.colors[last].b = fmax(fmin(0.01 / fabs(dist), 1.0), 0.0);
433  }
434  else
435  {
436  plane_marker.colors[last].b = fmax(fmin(0.1 / (dist + 0.001), 1.0), 0.0);
437  plane_marker.colors[last].g = fmax(fmin(0.05 / (dist + 0.001), 1.0), 0.0);
438  plane_marker.colors[last].r = fmax(fmin(0.01 / (dist + 0.001), 1.0), 0.0);
439  }
440  }
441  }
442  }
443 }
444 
445 void DistanceField::setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point,
446  std_msgs::ColorRGBA& color, double max_distance) const
447 {
448  double wx, wy, wz;
449  gridToWorld(xCell, yCell, zCell, wx, wy, wz);
450 
451  point.x = wx;
452  point.y = wy;
453  point.z = wz;
454 
455  color.r = 1.0;
456  color.g = dist / max_distance; // dist/max_distance * 0.5;
457  color.b = dist / max_distance; // dist/max_distance * 0.1;
458 }
459 
460 void DistanceField::getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_dist,
461  visualization_msgs::Marker& marker) const
462 {
463  int maxXCell = getXNumCells();
464  int maxYCell = getYNumCells();
465  int maxZCell = getZNumCells();
466 
467  double* x_projection = new double[maxYCell * maxZCell];
468  double* y_projection = new double[maxZCell * maxXCell];
469  double* z_projection = new double[maxXCell * maxYCell];
470  double initial_val = sqrt(INT_MAX);
471 
472  // Initialize
473  for (int y = 0; y < maxYCell; y++)
474  for (int x = 0; x < maxXCell; x++)
475  z_projection[x + y * maxXCell] = initial_val;
476 
477  for (int z = 0; z < maxZCell; z++)
478  for (int y = 0; y < maxYCell; y++)
479  x_projection[y + z * maxYCell] = initial_val;
480 
481  for (int z = 0; z < maxZCell; z++)
482  for (int x = 0; x < maxXCell; x++)
483  y_projection[x + z * maxXCell] = initial_val;
484 
485  // Calculate projections
486  for (int z = 0; z < maxZCell; z++)
487  {
488  for (int y = 0; y < maxYCell; y++)
489  {
490  for (int x = 0; x < maxXCell; x++)
491  {
492  double dist = getDistance(x, y, z);
493  z_projection[x + y * maxXCell] = std::min(dist, z_projection[x + y * maxXCell]);
494  x_projection[y + z * maxYCell] = std::min(dist, x_projection[y + z * maxYCell]);
495  y_projection[x + z * maxXCell] = std::min(dist, y_projection[x + z * maxXCell]);
496  }
497  }
498  }
499 
500  // Make markers
501  marker.points.clear();
502  marker.header.frame_id = frame_id;
503  marker.header.stamp = stamp;
504  marker.ns = "distance_field_projection_plane";
505  marker.id = 1;
506  marker.type = visualization_msgs::Marker::CUBE_LIST;
507  marker.action = visualization_msgs::Marker::MODIFY;
508  marker.scale.x = getResolution();
509  marker.scale.y = getResolution();
510  marker.scale.z = getResolution();
511  marker.color.a = 1.0;
512  // marker.lifetime = ros::Duration(30.0);
513 
514  int x, y, z;
515  int index = 0;
516  marker.points.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
517  marker.colors.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
518 
519  z = 0;
520  for (y = 0; y < maxYCell; y++)
521  {
522  for (x = 0; x < maxXCell; x++)
523  {
524  double dist = z_projection[x + y * maxXCell];
525  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
526  index++;
527  }
528  }
529 
530  x = 0;
531  for (z = 0; z < maxZCell; z++)
532  {
533  for (y = 0; y < maxYCell; y++)
534  {
535  double dist = x_projection[y + z * maxYCell];
536  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
537  index++;
538  }
539  }
540 
541  y = 0;
542  for (z = 0; z < maxZCell; z++)
543  {
544  for (x = 0; x < maxXCell; x++)
545  {
546  double dist = y_projection[x + z * maxXCell];
547  setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
548  index++;
549  }
550  }
551 
552  if (x_projection)
553  delete[] x_projection;
554  if (y_projection)
555  delete[] y_projection;
556  if (z_projection)
557  delete[] z_projection;
558 }
559 
560 } // end of namespace distance_field
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.
#define ROS_WARN_NAMED(name,...)
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.
Namespace for holding classes that generate distance fields.
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...
#define ROS_ERROR_NAMED(name,...)
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 Sun Oct 18 2020 13:16:33