55 , resolution_(resolution)
56 , inv_twice_resolution_(1.0 / (2.0 * resolution_))
63 double& gradient_z,
bool& in_bounds)
const 89 const ros::Time stamp, visualization_msgs::Marker& inf_marker)
const 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";
96 inf_marker.type = visualization_msgs::Marker::CUBE_LIST;
97 inf_marker.action = visualization_msgs::Marker::MODIFY;
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;
107 inf_marker.points.reserve(100000);
116 if (dist >= min_distance && dist <= max_distance)
118 int last = inf_marker.points.size();
119 inf_marker.points.resize(last + 1);
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();
133 const ros::Time& stamp, visualization_msgs::MarkerArray& marker_array)
const 135 Eigen::Vector3d unitX(1, 0, 0);
136 Eigen::Vector3d unitY(0, 1, 0);
137 Eigen::Vector3d unitZ(0, 0, 1);
147 double worldX, worldY, worldZ;
150 double gradientX, gradientY, gradientZ;
153 Eigen::Vector3d gradient(gradientX, gradientY, gradientZ);
155 if (in_bounds && distance >= min_distance && distance <= max_distance && gradient.norm() > 0)
157 visualization_msgs::Marker marker;
159 marker.header.frame_id = frame_id;
160 marker.header.stamp = stamp;
162 marker.ns =
"distance_field_gradient";
164 marker.type = visualization_msgs::Marker::ARROW;
165 marker.action = visualization_msgs::Marker::ADD;
167 marker.pose.position.x = worldX;
168 marker.pose.position.y = worldY;
169 marker.pose.position.z = worldZ;
184 marker.color.r = 0.0;
185 marker.color.g = 0.0;
186 marker.color.b = 1.0;
187 marker.color.a = 1.0;
189 marker_array.markers.push_back(marker);
204 ROS_ERROR_NAMED(
"distance_field",
"Problem dynamic casting shape that claims to be OcTree");
229 Eigen::Affine3d pose_e;
237 double min_x, min_y, min_z;
247 double max_x, max_y, max_z;
248 gridToWorld(num_x, num_y, num_z, max_x, max_y, max_z);
259 Eigen::Vector3d point(it.getX(), it.getY(), it.getZ());
260 points->push_back(point);
265 for (
double x = it.getX() - ceil_val;
x <= it.getX() + ceil_val;
x +=
resolution_)
267 for (
double y = it.getY() - ceil_val;
y <= it.getY() + ceil_val;
y +=
resolution_)
269 for (
double z = it.getZ() - ceil_val;
z <= it.getZ() + ceil_val;
z +=
resolution_)
271 points->push_back(Eigen::Vector3d(
x,
y,
z));
288 const Eigen::Affine3d& new_pose)
292 ROS_WARN_NAMED(
"distance_field",
"Move shape not supported for Octree");
308 const geometry_msgs::Pose& new_pose)
310 Eigen::Affine3d old_pose_e, new_pose_e;
329 Eigen::Affine3d pose_e;
335 const Eigen::Vector3d& origin,
const std::string& frame_id,
const ros::Time stamp,
336 visualization_msgs::Marker& plane_marker)
const 338 plane_marker.header.frame_id = frame_id;
339 plane_marker.header.stamp = stamp;
340 plane_marker.ns =
"distance_field_plane";
342 plane_marker.type = visualization_msgs::Marker::CUBE_LIST;
343 plane_marker.action = visualization_msgs::Marker::ADD;
349 plane_marker.points.reserve(100000);
350 plane_marker.colors.reserve(100000);
365 minX = -length / 2.0;
374 minX = -length / 2.0;
383 minY = -length / 2.0;
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)
410 for (
int y = minYCell;
y <= maxYCell; ++
y)
412 for (
int z = minZCell;
z <= maxZCell; ++
z)
419 int last = plane_marker.points.size();
420 plane_marker.points.resize(last + 1);
421 plane_marker.colors.resize(last + 1);
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();
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);
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);
446 std_msgs::ColorRGBA& color,
double max_distance)
const 456 color.g = dist / max_distance;
457 color.b = dist / max_distance;
461 visualization_msgs::Marker& marker)
const 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);
473 for (
int y = 0;
y < maxYCell;
y++)
474 for (
int x = 0;
x < maxXCell;
x++)
475 z_projection[
x +
y * maxXCell] = initial_val;
477 for (
int z = 0;
z < maxZCell;
z++)
478 for (
int y = 0;
y < maxYCell;
y++)
479 x_projection[
y +
z * maxYCell] = initial_val;
481 for (
int z = 0;
z < maxZCell;
z++)
482 for (
int x = 0;
x < maxXCell;
x++)
483 y_projection[
x +
z * maxXCell] = initial_val;
486 for (
int z = 0;
z < maxZCell;
z++)
488 for (
int y = 0;
y < maxYCell;
y++)
490 for (
int x = 0;
x < maxXCell;
x++)
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]);
501 marker.points.clear();
502 marker.header.frame_id = frame_id;
503 marker.header.stamp = stamp;
504 marker.ns =
"distance_field_projection_plane";
506 marker.type = visualization_msgs::Marker::CUBE_LIST;
507 marker.action = visualization_msgs::Marker::MODIFY;
511 marker.color.a = 1.0;
516 marker.points.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
517 marker.colors.resize(maxXCell * maxYCell + maxYCell * maxZCell + maxZCell * maxXCell);
520 for (y = 0; y < maxYCell; y++)
522 for (x = 0; x < maxXCell; x++)
524 double dist = z_projection[x + y * maxXCell];
525 setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
531 for (z = 0; z < maxZCell; z++)
533 for (y = 0; y < maxYCell; y++)
535 double dist = x_projection[y + z * maxYCell];
536 setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
542 for (z = 0; z < maxZCell; z++)
544 for (x = 0; x < maxXCell; x++)
546 double dist = y_projection[x + z * maxXCell];
547 setPoint(x, y, z, dist, marker.points[index], marker.colors[index], max_dist);
553 delete[] x_projection;
555 delete[] y_projection;
557 delete[] z_projection;
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)
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...
std::shared_ptr< const octomap::OcTree > octree
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.
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 ...
const leaf_bbx_iterator end_leafs_bbx() const
double resolution_
Resolution of the distance field.
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)
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