DistanceField is an abstract base class for computing distances from sets of 3D obstacle points. The distance assigned to a freespace cell should be the distance to the closest obstacle cell. Cells that are obstacle cells should either be marked as zero distance, or may have a negative distance if a signed version of the distance field is being used and an obstacle point is internal to an obstacle volume. More...

#include <distance_field.h>

Inheritance diagram for distance_field::DistanceField:

[legend]

Public Member Functions
	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. More...

virtual	~DistanceField ()

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. More...

virtual void	removePointsFromField (const EigenSTL::vector_Vector3d &points)=0
	Remove a set of obstacle points from the distance field, updating distance values accordingly. More...

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, and add any obstacle points that are in the new block set but not the old block set. Any points that are in both sets are left unchanged. More...

bool	getShapePoints (const shapes::Shape shape, const Eigen::Isometry3d &pose, EigenSTL::vector_Vector3d points)
	Get the points associated with a shape. This is mainly used when the external application needs to cache points. More...

void	addShapeToField (const shapes::Shape *shape, const Eigen::Isometry3d &pose)
	Adds the set of points corresponding to the shape at the given pose as obstacle points into the distance field. If the shape is an OcTree, the pose information is ignored and the OcTree is passed to the addOcTreeToField function. More...

void	addShapeToField (const shapes::Shape *shape, const geometry_msgs::msg::Pose &pose)

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 grid are added to the distance field. The octree can represent either a larger or smaller volume than the distance field. If the resolution of the octree is less than or equal to the resolution of the distance field then the center of each leaf cell of the octree will be added to the distance field. If the resolution of the octree is greater than a 3D volume of the correct resolution will be added for each occupied leaf node. More...

void	moveShapeInField (const shapes::Shape *shape, const Eigen::Isometry3d &old_pose, const Eigen::Isometry3d &new_pose)
	Moves the shape in the distance field from the old pose to the new pose, removing points that are no longer obstacle points, and adding points that are now obstacle points at the new pose. This function will discretize both shapes, and call the updatePointsInField function on the old and new point sets. More...

void	moveShapeInField (const shapes::Shape *shape, const geometry_msgs::msg::Pose &old_pose, const geometry_msgs::msg::Pose &new_pose)

void	removeShapeFromField (const shapes::Shape *shape, const Eigen::Isometry3d &pose)
	All points corresponding to the shape are removed from the distance field. More...

void	removeShapeFromField (const shapes::Shape *shape, const geometry_msgs::msg::Pose &pose)

virtual void	reset ()=0
	Resets all points in the distance field to an uninitialize value. More...

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 heavily dependent on the behavior of the derived class. More...

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 as a function of the distances of near-by cells. An uninitialized distance is returned if the cell is not valid for gradient production purposes. The gradient is pointing out of the obstacle - thus to recover the closest obstacle point, the normalized gradient value is multiplied by the distance and subtracted from the cell's location, as shown below. More...

virtual double	getDistance (int x, int y, int z) const =0
	Gets the distance to the closest obstacle at the given integer cell location. The particulars of this function are heavily dependent on the behavior of the derived class. More...

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 field. More...

virtual int	getXNumCells () const =0
	Gets the number of cells along the X axis. More...

virtual int	getYNumCells () const =0
	Gets the number of cells along the Y axis. More...

virtual int	getZNumCells () const =0
	Gets the number of cells along the Z axis. More...

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 parameters. More...

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 is not valid in the distance field, and should populate the index values in either case. More...

virtual bool	writeToStream (std::ostream &stream) const =0
	Writes the contents of the distance field to the supplied stream. More...

virtual bool	readFromStream (std::istream &stream)=0
	Reads, parameterizes, and populates the distance field based on the supplied stream. More...

void	getIsoSurfaceMarkers (double min_distance, double max_distance, const std::string &frame_id, const rclcpp::Time &stamp, visualization_msgs::msg::Marker &marker) const
	Get an iso-surface for visualization in rviz. The iso-surface shows every cell that has a distance in a given range in the distance field. The cells are added as a visualization_msgs::msg::Marker::CUBE_LIST in the namespace "distance_field". More...

void	getGradientMarkers (double min_radius, double max_radius, const std::string &frame_id, const rclcpp::Time &stamp, visualization_msgs::msg::MarkerArray &marker_array) const
	Populates the supplied marker array with a series of arrows representing gradients of cells that are within the supplied range in terms of distance. The markers will be visualization_msgs::msg::Marker::ARROW in the namespace "distance_field_gradient". More...

void	getPlaneMarkers (PlaneVisualizationType type, double length, double width, double height, const Eigen::Vector3d &origin, const std::string &frame_id, const rclcpp::Time &stamp, visualization_msgs::msg::Marker &marker) const
	Populates a marker with a slice of the distance field in a particular plane. All cells in the plane will be added to the field, with colors associated with their distance. More...

void	getProjectionPlanes (const std::string &frame_id, const rclcpp::Time &stamp, double max_distance, visualization_msgs::msg::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. More...

double	getSizeX () const
	Gets the distance field size along the X dimension in meters. More...

double	getSizeY () const
	Gets the distance field size along the Y dimension in meters. More...

double	getSizeZ () const
	Gets the distance field size along the Z dimension in meters. More...

double	getOriginX () const
	Gets the origin (minimum value) along the X dimension. More...

double	getOriginY () const
	Gets the origin (minimum value) along the Y dimension. More...

double	getOriginZ () const
	Gets the origin (minimum value) along the Z dimension. More...

double	getResolution () const
	Gets the resolution of the distance field in meters. More...

virtual double	getUninitializedDistance () const =0
	Gets a distance value for an invalid cell. More...

Protected Member Functions
void	getOcTreePoints (const octomap::OcTree octree, EigenSTL::vector_Vector3d points)
	Get the points associated with an octree. More...

void	setPoint (int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point &point, std_msgs::msg::ColorRGBA &color, double max_distance) const
	Helper function that sets the point value and color given the distance. More...

Protected Attributes
double	size_x_
	X size of the distance field. More...

double	size_y_
	Y size of the distance field. More...

double	size_z_
	Z size of the distance field. More...

double	origin_x_
	X origin of the distance field. More...

double	origin_y_
	Y origin of the distance field. More...

double	origin_z_
	Z origin of the distance field. More...

double	resolution_
	Resolution of the distance field. More...

int	inv_twice_resolution_
	Computed value 1.0/(2.0*resolution_) More...

Detailed Description

DistanceField is an abstract base class for computing distances from sets of 3D obstacle points. The distance assigned to a freespace cell should be the distance to the closest obstacle cell. Cells that are obstacle cells should either be marked as zero distance, or may have a negative distance if a signed version of the distance field is being used and an obstacle point is internal to an obstacle volume.

Inherited classes must contain methods for holding a dense set of 3D voxels as well as methods for computing the required distances. The distance field parent class doesn't hold the data or have any way to generate distances from that data.

Definition at line 92 of file distance_field.h.

Constructor & Destructor Documentation

◆ DistanceField()

distance_field::DistanceField::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.

Parameters

[in]	size_x	The X dimension in meters of the volume to represent
[in]	size_y	The Y dimension in meters of the volume to represent
[in]	size_z	The Z dimension in meters of the volume to represent
[in]	resolution	The resolution in meters of the volume
[in]	origin_x	The minimum X point of the volume
[in]	origin_y	The minimum Y point of the volume
[in]	origin_z	The minimum Z point of the volume

Definition at line 52 of file distance_field.cpp.

◆ ~DistanceField()

distance_field::DistanceField::~DistanceField ( )

virtualdefault

Member Function Documentation

◆ addOcTreeToField()

void distance_field::DistanceField::addOcTreeToField ( const octomap::OcTree * octree )

Adds an octree to the distance field. Cells that are occupied in the octree that lie within the voxel grid are added to the distance field. The octree can represent either a larger or smaller volume than the distance field. If the resolution of the octree is less than or equal to the resolution of the distance field then the center of each leaf cell of the octree will be added to the distance field. If the resolution of the octree is greater than a 3D volume of the correct resolution will be added for each occupied leaf node.

Parameters

[in] octree The octree to add to the distance field

Definition at line 288 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ addPointsToField()

virtual void distance_field::DistanceField::addPointsToField ( const EigenSTL::vector_Vector3d & points )

pure virtual

Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells.

Parameters

[in] points The set of obstacle points to add

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ addShapeToField() [1/2]

void distance_field::DistanceField::addShapeToField	(	const shapes::Shape *	shape,
		const Eigen::Isometry3d &	pose
	)

Adds the set of points corresponding to the shape at the given pose as obstacle points into the distance field. If the shape is an OcTree, the pose information is ignored and the OcTree is passed to the addOcTreeToField function.

This function uses the Body class in the geometric_shapes package to determine the set of obstacle points, with the exception of OcTrees as mentioned. A bounding sphere is computed given the shape; the bounding sphere is iterated through in 3D at the resolution of the distance_field, with each point tested for point inclusion. For more information about the behavior of bodies and poses please see the documentation for geometric_shapes.

Parameters

[in]	shape	The shape to add to the distance field
[in]	pose	The pose of the shape

Definition at line 227 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ addShapeToField() [2/2]

void distance_field::DistanceField::addShapeToField	(	const shapes::Shape *	shape,
		const geometry_msgs::msg::Pose &	pose
	)

Definition at line 235 of file distance_field.cpp.

Here is the call graph for this function:

◆ getDistance() [1/2]

virtual double distance_field::DistanceField::getDistance	(	double	x,
		double	y,
		double	z
	)		const

pure virtual

Gets the distance to the closest obstacle at the given location. The particulars of this function are heavily dependent on the behavior of the derived class.

The particular implementation may return a max distance value if a cell is far away from all obstacles. Values of 0.0 should represent that a cell is an obstacle, and some implementations may return a signed value representing distance to the surface when a cell is deep inside an obstacle volume. An implementation may also return some value to represent when a location is outside the represented volume.

Parameters

[in]	x	The cell's X value
[in]	y	The cell's Y value
[in]	z	The cell's Z value

Returns: The distance to the closest obstacle cell

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ getDistance() [2/2]

virtual double distance_field::DistanceField::getDistance	(	int	x,
		int	y,
		int	z
	)		const

pure virtual

Gets the distance to the closest obstacle at the given integer cell location. The particulars of this function are heavily dependent on the behavior of the derived class.

Parameters

[in]	x	The X index of the cell
[in]	y	The Y index of the cell
[in]	z	The Z index of the cell

Returns: The distance to the closest occupied cell

Implemented in distance_field::PropagationDistanceField.

◆ getDistanceGradient()

double distance_field::DistanceField::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 as a function of the distances of near-by cells. An uninitialized distance is returned if the cell is not valid for gradient production purposes. The gradient is pointing out of the obstacle - thus to recover the closest obstacle point, the normalized gradient value is multiplied by the distance and subtracted from the cell's location, as shown below.

A number of different cells will not have valid gradients. Any cell that is entirely surrounded by cells of the same distance will not have a valid gradient. Depending on the implementation of the distance field, such cells may be found far away from obstacles (if a distance is not computed for every cell), or deep within obstacles. Such points can be detected as having zero magnitude for the gradient.

The closest cell to a given cell can be computed given the following formulation (this value will only be within the resolution parameter of the correct location), including a test for a non-zero gradient magnitude:

Eigen::Vector3d grad;
double dist = distance_field.getDistanceGradient(x,y,z,grad.x(),grad.y(),grad.z(),in_bounds);
if(grad.norm() > 0) {
  double closest_point_x = x-(grad.x()/grad.norm())*dist;
  double closest_point_y = y-(grad.y()/grad.norm())*dist;
  double closest_point_z = z-(grad.z()/grad.norm())*dist;
}

Parameters

[in]	x	The X location of the cell
[in]	y	The X location of the cell
[in]	z	The X location of the cell
[out]	gradient_x	The X component of the gradient to the closest occupied cell
[out]	gradient_y	The Y component of the gradient to the closest occupied cell
[out]	gradient_z	The Z component of the gradient to the closest occupied cell
[out]	in_bounds	Whether or not the (x,y,z) is valid for gradient purposes. Gradients are not valid at the boundary of the distance field (cells where one or more of the indexes are at 0 or at the maximum size).

Returns: The distance to the closest occupied cell

Definition at line 67 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getGradientMarkers()

void distance_field::DistanceField::getGradientMarkers	(	double	min_radius,
		double	max_radius,
		const std::string &	frame_id,
		const rclcpp::Time &	stamp,
		visualization_msgs::msg::MarkerArray &	marker_array
	)		const

Populates the supplied marker array with a series of arrows representing gradients of cells that are within the supplied range in terms of distance. The markers will be visualization_msgs::msg::Marker::ARROW in the namespace "distance_field_gradient".

Parameters

[in]	min_distance	Cells of less than this distance will not be added to the marker
[in]	max_distance	Cells of greater than this distance will not be added to the marker
[in]	frame_id	The frame to use as the header in the marker
[in]	stamp	The stamp to use in the header of the marker
[out]	marker_array	The marker array to populate

Definition at line 137 of file distance_field.cpp.

Here is the call graph for this function:

◆ getIsoSurfaceMarkers()

void distance_field::DistanceField::getIsoSurfaceMarkers	(	double	min_distance,
		double	max_distance,
		const std::string &	frame_id,
		const rclcpp::Time &	stamp,
		visualization_msgs::msg::Marker &	marker
	)		const

Get an iso-surface for visualization in rviz. The iso-surface shows every cell that has a distance in a given range in the distance field. The cells are added as a visualization_msgs::msg::Marker::CUBE_LIST in the namespace "distance_field".

Parameters

[in]	min_distance	Cells of less than this distance will not be added to the marker
[in]	max_distance	Cells of greater than this distance will not be added to the marker
[in]	frame_id	The frame to use as the header in the marker
[in]	stamp	The stamp to use in the header of the marker
[out]	marker	The marker that will contain the indicated cells.

Definition at line 93 of file distance_field.cpp.

Here is the call graph for this function:

◆ getOcTreePoints()

void distance_field::DistanceField::getOcTreePoints	(	const octomap::OcTree *	octree,
		EigenSTL::vector_Vector3d *	points
	)

protected

Get the points associated with an octree.

Parameters

[in]	octree	The octree to find points for.
[out]	points	The points determined for this octree.

Definition at line 242 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getOriginX()

double distance_field::DistanceField::getOriginX ( ) const

inline

Gets the origin (minimum value) along the X dimension.

Returns: The X origin

Definition at line 544 of file distance_field.h.

◆ getOriginY()

double distance_field::DistanceField::getOriginY ( ) const

inline

Gets the origin (minimum value) along the Y dimension.

Returns: The Y origin

Definition at line 555 of file distance_field.h.

◆ getOriginZ()

double distance_field::DistanceField::getOriginZ ( ) const

inline

Gets the origin (minimum value) along the Z dimension.

Returns: The Z origin

Definition at line 566 of file distance_field.h.

◆ getPlaneMarkers()

void distance_field::DistanceField::getPlaneMarkers	(	PlaneVisualizationType	type,
		double	length,
		double	width,
		double	height,
		const Eigen::Vector3d &	origin,
		const std::string &	frame_id,
		const rclcpp::Time &	stamp,
		visualization_msgs::msg::Marker &	marker
	)		const

Populates a marker with a slice of the distance field in a particular plane. All cells in the plane will be added to the field, with colors associated with their distance.

Parameters

[in]	type	Which plane to show in the marker
[in]	length	The length of the plane to show. If the type is XZ or XY, it's interpreted as the dimension along the X axis. If the type is YZ, it's interpreted along the Y axis.
[in]	width	The width of the plane to show. If the type is XZ or YZ, it's interpreted along the Z axis. If the type is XY, it's interpreted along the Y axis.
[in]	height	The height of the plane to show. If the type is XY, it's interpreted along the Z axis. If the type is XZ, it's interpreted along the Y axis. If the type is YZ, it's interpreted along the X axis.
[in]	origin	The minimum point along each axis to display
[in]	frame_id	The frame to use as the header in the marker
[in]	stamp	The stamp to use in the header of the marker
[out]	marker	The marker that will contain the indicated cells.

Definition at line 346 of file distance_field.cpp.

Here is the call graph for this function:

◆ getProjectionPlanes()

void distance_field::DistanceField::getProjectionPlanes	(	const std::string &	frame_id,
		const rclcpp::Time &	stamp,
		double	max_distance,
		visualization_msgs::msg::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.

Parameters

[in]	frame_id	The frame to use as the header in the marker
[in]	stamp	The stamp to use in the header of the marker
[in]	max_distance	A max distance for color calculation. Distances of this value or greater will show up as fully white in the marker.
[out]	marker	The marker, which will be populated with a visualization_msgs::msg::Marker::CUBE_LIST .

Definition at line 472 of file distance_field.cpp.

Here is the call graph for this function:

◆ getResolution()

double distance_field::DistanceField::getResolution ( ) const

inline

Gets the resolution of the distance field in meters.

Returns: The resolution of the distance field in meters

Definition at line 577 of file distance_field.h.

Here is the caller graph for this function:

◆ getShapePoints()

bool distance_field::DistanceField::getShapePoints	(	const shapes::Shape *	shape,
		const Eigen::Isometry3d &	pose,
		EigenSTL::vector_Vector3d *	points
	)

Get the points associated with a shape. This is mainly used when the external application needs to cache points.

Parameters

[in]	shape	The shape to find points for.
[in]	pose	The pose of the shape.
[out]	points	The points determined for this shape.

Definition at line 202 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ getSizeX()

double distance_field::DistanceField::getSizeX ( ) const

inline

Gets the distance field size along the X dimension in meters.

Returns: The size along the X dimension in meters

Definition at line 511 of file distance_field.h.

◆ getSizeY()

double distance_field::DistanceField::getSizeY ( ) const

inline

Gets the distance field size along the Y dimension in meters.

Returns: The size along the Y dimension in meters

Definition at line 522 of file distance_field.h.

◆ getSizeZ()

double distance_field::DistanceField::getSizeZ ( ) const

inline

Gets the distance field size along the Z dimension in meters.

Returns: The size along the Z dimension in meters

Definition at line 533 of file distance_field.h.

◆ getUninitializedDistance()

virtual double distance_field::DistanceField::getUninitializedDistance ( ) const

pure virtual

Gets a distance value for an invalid cell.

Returns: The distance associated with an uninitialized cell

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ getXNumCells()

virtual int distance_field::DistanceField::getXNumCells ( ) const

pure virtual

Gets the number of cells along the X axis.

Returns: The number of cells along the X axis

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ getYNumCells()

virtual int distance_field::DistanceField::getYNumCells ( ) const

pure virtual

Gets the number of cells along the Y axis.

Returns: The number of cells along the Y axis

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ getZNumCells()

virtual int distance_field::DistanceField::getZNumCells ( ) const

pure virtual

Gets the number of cells along the Z axis.

Returns: The number of cells along the Z axis

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ gridToWorld()

virtual bool distance_field::DistanceField::gridToWorld	(	int	x,
		int	y,
		int	z,
		double &	world_x,
		double &	world_y,
		double &	world_z
	)		const

pure virtual

Converts from an set of integer indices to a world location given the origin and resolution parameters.

Parameters

[in]	x	The integer X location
[in]	y	The integer Y location
[in]	z	The integer Z location
[out]	world_x	The computed world X location
[out]	world_y	The computed world X location
[out]	world_z	The computed world X location

Returns: Whether or not the transformation is successful. An implementation may or may not choose to return false if the indicated cell is not valid for this distance field.

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ isCellValid()

virtual bool distance_field::DistanceField::isCellValid	(	int	x,
		int	y,
		int	z
	)		const

pure virtual

Determines whether or not the cell associated with the supplied indices is valid for this distance field.

Parameters

[in]	x	The X index of the cell
[in]	y	The Y index of the cell
[in]	z	The Z index of the cell

Returns: True if the cell is valid, otherwise false.

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ moveShapeInField() [1/2]

void distance_field::DistanceField::moveShapeInField	(	const shapes::Shape *	shape,
		const Eigen::Isometry3d &	old_pose,
		const Eigen::Isometry3d &	new_pose
	)

Moves the shape in the distance field from the old pose to the new pose, removing points that are no longer obstacle points, and adding points that are now obstacle points at the new pose. This function will discretize both shapes, and call the updatePointsInField function on the old and new point sets.

It's important to note that this function has no semantic understanding of an object - this function may be called even if addShapeToField was not previously called. Furthermore, points will be removed even if they have been added by multiple different sources - a cup resting on a table that is moved make take a chunk out of the top of the table.

Parameters

[in]	shape	The shape to move in the distance field
[in]	old_pose	The old pose of the shape
[in]	new_pose	The new pose of the shape

Definition at line 295 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ moveShapeInField() [2/2]

void distance_field::DistanceField::moveShapeInField	(	const shapes::Shape *	shape,
		const geometry_msgs::msg::Pose &	old_pose,
		const geometry_msgs::msg::Pose &	new_pose
	)

Definition at line 317 of file distance_field.cpp.

Here is the call graph for this function:

◆ readFromStream()

virtual bool distance_field::DistanceField::readFromStream ( std::istream & stream )

pure virtual

Reads, parameterizes, and populates the distance field based on the supplied stream.

Parameters

[in] stream The stream from which to read

Returns: True if reading, parameterizing, and populating the distance field is successful; otherwise False.

Implemented in distance_field::PropagationDistanceField.

◆ removePointsFromField()

virtual void distance_field::DistanceField::removePointsFromField ( const EigenSTL::vector_Vector3d & points )

pure virtual

Remove a set of obstacle points from the distance field, updating distance values accordingly.

This function will invalidate the distance measurements associated with the obstacle points and recompute them. Depending on the implementation of the derived class and the proportion of the total occupied points being removed, it may be more efficient to use the reset() function and add the remaining obstacle points to the grid again.

Parameters

[in] points The set of obstacle points that will be set as free

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ removeShapeFromField() [1/2]

void distance_field::DistanceField::removeShapeFromField	(	const shapes::Shape *	shape,
		const Eigen::Isometry3d &	pose
	)

All points corresponding to the shape are removed from the distance field.

The points needs not have been added using addShapeToField.

Parameters

[in]	shape	The shape to remove from the distance field
[in]	pose	The pose of the shape to remove

Definition at line 326 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ removeShapeFromField() [2/2]

void distance_field::DistanceField::removeShapeFromField	(	const shapes::Shape *	shape,
		const geometry_msgs::msg::Pose &	pose
	)

Definition at line 339 of file distance_field.cpp.

Here is the call graph for this function:

◆ reset()

virtual void distance_field::DistanceField::reset ( )

pure virtual

Resets all points in the distance field to an uninitialize value.

Implemented in distance_field::PropagationDistanceField.

◆ setPoint()

void distance_field::DistanceField::setPoint	(	int	xCell,
		int	yCell,
		int	zCell,
		double	dist,
		geometry_msgs::msg::Point &	point,
		std_msgs::msg::ColorRGBA &	color,
		double	max_distance
	)		const

protected

Helper function that sets the point value and color given the distance.

Parameters

[in]	xCell	The x index of the cell
[in]	yCell	The y index of the cell
[in]	zCell	The z index of the cell
[in]	dist	The distance of the cell
[out]	point	World coordinates will be placed here
[out]	color	A color will be assigned here that's only red if the distance is 0, and gets progressively whiter as the dist value approaches max_distance.
[in]	max_distance	The distance past which all cells will be fully white

Definition at line 457 of file distance_field.cpp.

Here is the call graph for this function:

Here is the caller graph for this function:

◆ updatePointsInField()

virtual void distance_field::DistanceField::updatePointsInField	(	const EigenSTL::vector_Vector3d &	old_points,
		const EigenSTL::vector_Vector3d &	new_points
	)

pure virtual

This function will remove any obstacle points that are in the old point set but not the new point set, and add any obstacle points that are in the new block set but not the old block set. Any points that are in both sets are left unchanged.

The primary use case for this function is in moving objects - calculating the set of points associated with an object, moving the object in space, and calculating the new set of points. If the object has moved substantially, such that the old object position does not overlap with the new object position, then it may be more efficient to call removePointsFromField on the old points and addPointsToField on the new points. If the object has moved only slightly, however, this function may offer a speed improvement. All points in the old_points set should have been previously added to the field in order for this function to act as intended - points that are in both the old_points set and the new_points set will not be added to the field, as they are assumed to already be obstacle points.

Parameters

[in]	old_points	The set of points that all should be obstacle cells in the distance field
[in]	new_points	The set of points, all of which are intended to be obstacle points in the distance field

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ worldToGrid()

virtual bool distance_field::DistanceField::worldToGrid	(	double	world_x,
		double	world_y,
		double	world_z,
		int &	x,
		int &	y,
		int &	z
	)		const

pure virtual

Converts from a world location to a set of integer indices. Should return false if the world location is not valid in the distance field, and should populate the index values in either case.

Parameters

[in]	world_x	The world X location
[in]	world_y	The world Y location
[in]	world_z	The world Z location
[out]	x	The computed integer X location
[out]	y	The computed integer X location
[out]	z	The computed integer X location

Returns: True if all the world values result in integer indices that pass a validity check; otherwise False.

Implemented in distance_field::PropagationDistanceField.

Here is the caller graph for this function:

◆ writeToStream()

virtual bool distance_field::DistanceField::writeToStream ( std::ostream & stream ) const

pure virtual

Writes the contents of the distance field to the supplied stream.

Parameters

[out] stream The stream to which to write the distance field contents.

Returns: True if the writing is successful; otherwise, false.

Implemented in distance_field::PropagationDistanceField.

Member Data Documentation

◆ inv_twice_resolution_

int distance_field::DistanceField::inv_twice_resolution_

protected

Computed value 1.0/(2.0*resolution_)

Definition at line 623 of file distance_field.h.

◆ origin_x_

double distance_field::DistanceField::origin_x_

protected

X origin of the distance field.

Definition at line 619 of file distance_field.h.

◆ origin_y_

double distance_field::DistanceField::origin_y_

protected

Y origin of the distance field.

Definition at line 620 of file distance_field.h.

◆ origin_z_

double distance_field::DistanceField::origin_z_

protected

Z origin of the distance field.

Definition at line 621 of file distance_field.h.

◆ resolution_

double distance_field::DistanceField::resolution_

protected

Resolution of the distance field.

Definition at line 622 of file distance_field.h.

◆ size_x_

double distance_field::DistanceField::size_x_

protected

X size of the distance field.

Definition at line 616 of file distance_field.h.

◆ size_y_

double distance_field::DistanceField::size_y_

protected

Y size of the distance field.

Definition at line 617 of file distance_field.h.

◆ size_z_

double distance_field::DistanceField::size_z_

protected

Z size of the distance field.

Definition at line 618 of file distance_field.h.

The documentation for this class was generated from the following files:

moveit_core/distance_field/include/moveit/distance_field/distance_field.h
moveit_core/distance_field/src/distance_field.cpp

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ DistanceField()

◆ ~DistanceField()

Member Function Documentation

◆ addOcTreeToField()

◆ addPointsToField()

◆ addShapeToField() [1/2]

◆ addShapeToField() [2/2]

◆ getDistance() [1/2]

◆ getDistance() [2/2]

◆ getDistanceGradient()

◆ getGradientMarkers()

◆ getIsoSurfaceMarkers()

◆ getOcTreePoints()

◆ getOriginX()

◆ getOriginY()

◆ getOriginZ()

◆ getPlaneMarkers()

◆ getProjectionPlanes()

◆ getResolution()

◆ getShapePoints()

◆ getSizeX()

◆ getSizeY()

◆ getSizeZ()

◆ getUninitializedDistance()

◆ getXNumCells()

◆ getYNumCells()

◆ getZNumCells()

◆ gridToWorld()

◆ isCellValid()

◆ moveShapeInField() [1/2]

◆ moveShapeInField() [2/2]

◆ readFromStream()

◆ removePointsFromField()

◆ removeShapeFromField() [1/2]

◆ removeShapeFromField() [2/2]

◆ reset()

◆ setPoint()

◆ updatePointsInField()

◆ worldToGrid()

◆ writeToStream()

Member Data Documentation

◆ inv_twice_resolution_

◆ origin_x_

◆ origin_y_

◆ origin_z_

◆ resolution_

◆ size_x_

◆ size_y_

◆ size_z_