moveit2
The MoveIt Motion Planning Framework for ROS 2.
Public Member Functions | Protected Attributes | List of all members
collision_detection::PosedDistanceField Class Reference

#include <collision_distance_field_types.h>

Inheritance diagram for collision_detection::PosedDistanceField:
Inheritance graph
[legend]
Collaboration diagram for collision_detection::PosedDistanceField:
Collaboration graph
[legend]

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW PosedDistanceField (const Eigen::Vector3d &size, const Eigen::Vector3d &origin, double resolution, double max_distance, bool propagate_negative_distances=false)
 
void updatePose (const Eigen::Isometry3d &transform)
 
const Eigen::Isometry3d & getPose () const
 
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 point defined by the x, y and z values is transform into the local distance field coordinate system using the current pose. 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. More...
 
bool getCollisionSphereGradients (const std::vector< CollisionSphere > &sphere_list, const EigenSTL::vector_Vector3d &sphere_centers, GradientInfo &gradient, const CollisionType &type, double tolerance, bool subtract_radii, double maximum_value, bool stop_at_first_collision)
 
- Public Member Functions inherited from distance_field::PropagationDistanceField
 PropagationDistanceField (double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y, double origin_z, double max_distance, bool propagate_negative_distances=false)
 Constructor that initializes entire distance field to empty - all cells will be assigned maximum distance values. All units are arbitrary but are assumed for documentation purposes to represent meters. More...
 
 PropagationDistanceField (const octomap::OcTree &octree, const octomap::point3d &bbx_min, const octomap::point3d &bbx_max, double max_distance, bool propagate_negative_distances=false)
 Constructor based on an OcTree and bounding box information. A distance field will be constructed with dimensions based on the supplied bounding box at the resolution of the OcTree. All octree obstacle cells will be added to the resulting distance field using the DistanceField::addOcTreeToField function. More...
 
 PropagationDistanceField (std::istream &stream, double max_distance, bool propagate_negative_distances=false)
 Constructor that takes an istream and reads the contents of a saved distance field, adding all obstacle points and running propagation given the arguments for max_distance and propagate_negative_distances. Calls the function readFromStream. More...
 
 ~PropagationDistanceField () override
 Empty destructor. More...
 
void addPointsToField (const EigenSTL::vector_Vector3d &points) override
 Add a set of obstacle points to the distance field, updating distance values accordingly. The distance field may already contain obstacle cells. More...
 
void removePointsFromField (const EigenSTL::vector_Vector3d &points) override
 Remove a set of obstacle points from the distance field, updating distance values accordingly. More...
 
void updatePointsInField (const EigenSTL::vector_Vector3d &old_points, const EigenSTL::vector_Vector3d &new_points) override
 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. For more information see DistanceField::updatePointsInField. More...
 
void reset () override
 Resets the entire distance field to max_distance for positive values and zero for negative values. More...
 
double getDistance (double x, double y, double z) const override
 Get the distance value associated with the cell indicated by the world coordinate. If the cell is invalid, max_distance will be returned. If running without negative distances, all obstacle cells will have zero distance. If running with negative distances, the distance will be between -max_distance and max_distance, with no values having a 0 distance. More...
 
double getDistance (int x, int y, int z) const override
 Get the distance value associated with the cell indicated by the index coordinates. If the cell is invalid, max_distance will be returned. If running without negative distances, all obstacle cells will have zero distance. If running with negative distances, the distance will be between -max_distance and max_distance, with no values having a 0 distance. More...
 
bool isCellValid (int x, int y, int z) const override
 Determines whether or not the cell associated with the supplied indices is valid for this distance field. More...
 
int getXNumCells () const override
 Gets the number of cells along the X axis. More...
 
int getYNumCells () const override
 Gets the number of cells along the Y axis. More...
 
int getZNumCells () const override
 Gets the number of cells along the Z axis. More...
 
bool gridToWorld (int x, int y, int z, double &world_x, double &world_y, double &world_z) const override
 Converts from an set of integer indices to a world location given the origin and resolution parameters. More...
 
bool worldToGrid (double world_x, double world_y, double world_z, int &x, int &y, int &z) const override
 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...
 
bool writeToStream (std::ostream &stream) const override
 Writes the contents of the distance field to the supplied stream. More...
 
bool readFromStream (std::istream &stream) override
 Reads, parameterizes, and populates the distance field based on the supplied stream. More...
 
double getUninitializedDistance () const override
 Gets a distance value for an invalid cell. More...
 
const PropDistanceFieldVoxelgetCell (int x, int y, int z) const
 Gets full cell data given an index. More...
 
const PropDistanceFieldVoxelgetNearestCell (int x, int y, int z, double &dist, Eigen::Vector3i &pos) const
 Gets nearest surface cell and returns distance to it. More...
 
int getMaximumDistanceSquared () const
 Gets the maximum distance squared value. More...
 
- Public Member Functions inherited from 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. More...
 
virtual ~DistanceField ()
 
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)
 
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...
 
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...
 

Protected Attributes

Eigen::Isometry3d pose_
 
- Protected Attributes inherited from distance_field::DistanceField
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...
 

Additional Inherited Members

- Protected Member Functions inherited from distance_field::DistanceField
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...
 

Detailed Description

Definition at line 120 of file collision_distance_field_types.h.

Constructor & Destructor Documentation

◆ PosedDistanceField()

EIGEN_MAKE_ALIGNED_OPERATOR_NEW collision_detection::PosedDistanceField::PosedDistanceField ( const Eigen::Vector3d &  size,
const Eigen::Vector3d &  origin,
double  resolution,
double  max_distance,
bool  propagate_negative_distances = false 
)
inline

Definition at line 125 of file collision_distance_field_types.h.

Member Function Documentation

◆ getCollisionSphereGradients()

bool collision_detection::PosedDistanceField::getCollisionSphereGradients ( const std::vector< CollisionSphere > &  sphere_list,
const EigenSTL::vector_Vector3d &  sphere_centers,
GradientInfo gradient,
const CollisionType type,
double  tolerance,
bool  subtract_radii,
double  maximum_value,
bool  stop_at_first_collision 
)

Definition at line 81 of file collision_distance_field_types.cpp.

Here is the call graph for this function:

◆ getDistanceGradient()

double collision_detection::PosedDistanceField::getDistanceGradient ( double  x,
double  y,
double  z,
double &  gradient_x,
double &  gradient_y,
double &  gradient_z,
bool &  in_bounds 
) const
inline

Gets not only the distance to the nearest cell but the gradient direction. The point defined by the x, y and z values is transform into the local distance field coordinate system using the current pose. 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.

Parameters
xinput x value of the query point
yinput y value of the query point
zinput z value of the query point
gradient_xoutput argument with the x value of the gradient
gradient_youtput argument with the y value of the gradient
gradient_zoutput argument with the z value of the gradient
in_boundstrue if the point is within the bounds of the distance field, false otherwise

Definition at line 162 of file collision_distance_field_types.h.

Here is the call graph for this function:
Here is the caller graph for this function:

◆ getPose()

const Eigen::Isometry3d& collision_detection::PosedDistanceField::getPose ( ) const
inline

Definition at line 138 of file collision_distance_field_types.h.

◆ updatePose()

void collision_detection::PosedDistanceField::updatePose ( const Eigen::Isometry3d &  transform)
inline

Definition at line 133 of file collision_distance_field_types.h.

Member Data Documentation

◆ pose_

Eigen::Isometry3d collision_detection::PosedDistanceField::pose_
protected

Definition at line 198 of file collision_distance_field_types.h.


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