Go to the documentation of this file.
41 #include <visualization_msgs/Marker.h>
42 #include <visualization_msgs/MarkerArray.h>
44 #include <Eigen/Geometry>
106 DistanceField(
double size_x,
double size_y,
double size_z,
double resolution,
double origin_x,
double origin_y,
227 const Eigen::Isometry3d& new_pose);
231 const geometry_msgs::Pose& new_pose);
251 virtual void reset() = 0;
272 virtual double getDistance(
double x,
double y,
double z)
const = 0;
321 double getDistanceGradient(
double x,
double y,
double z,
double& gradient_x,
double& gradient_y,
double& gradient_z,
322 bool& in_bounds)
const;
385 virtual bool gridToWorld(
int x,
int y,
int z,
double& world_x,
double& world_y,
double& world_z)
const = 0;
403 virtual bool worldToGrid(
double world_x,
double world_y,
double world_z,
int&
x,
int&
y,
int&
z)
const = 0;
438 void getIsoSurfaceMarkers(
double min_distance,
double max_distance,
const std::string& frame_id,
439 const ros::Time stamp, visualization_msgs::Marker& marker)
const;
455 visualization_msgs::MarkerArray& marker_array)
const;
484 visualization_msgs::Marker& marker)
const;
502 visualization_msgs::Marker& marker)
const;
612 void setPoint(
int xCell,
int yCell,
int zCell,
double dist, geometry_msgs::Point&
point, std_msgs::ColorRGBA& color,
613 double max_distance)
const;
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...
double getResolution() const
Gets the resolution of the distance field in meters.
virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)=0
Remove a set of obstacle points from the distance field, updating distance values accordingly.
double origin_z_
Z origin of the distance field.
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...
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 ...
virtual bool writeToStream(std::ostream &stream) const =0
Writes the contents of the distance field to the supplied stream.
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...
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...
double origin_x_
X origin of the distance field.
MOVEIT_CLASS_FORWARD(Shape)
MOVEIT_CLASS_FORWARD(DistanceField)
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.
PlaneVisualizationType
The plane to visualize.
double size_y_
Y size of the distance field.
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 dista...
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...
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
void removeShapeFromField(const shapes::Shape *shape, const Eigen::Isometry3d &pose)
All points corresponding to the shape are removed from the distance field.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
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...
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 ca...
double getOriginY() const
Gets the origin (minimum value) along the Y dimension.
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...
void getOcTreePoints(const octomap::OcTree *octree, EigenSTL::vector_Vector3d *points)
Get the points associated with an octree.
double resolution_
Resolution of the distance field.
virtual int getYNumCells() const =0
Gets the number of cells along the Y axis.
double getSizeZ() const
Gets the distance field size along the Z dimension in meters.
double size_x_
X size of the distance field.
Namespace for holding classes that generate distance fields.
std::chrono::system_clock::time_point point
virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)=0
Add a set of obstacle points to the distance field, updating distance values accordingly....
double origin_y_
Y origin of the distance field.
double getOriginZ() const
Gets the origin (minimum value) along the Z dimension.
virtual bool readFromStream(std::istream &stream)=0
Reads, parameterizes, and populates the distance field based on the supplied stream.
int inv_twice_resolution_
Computed value 1.0/(2.0*resolution_)
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.
virtual double getUninitializedDistance() const =0
Gets a distance value for an invalid cell.
double size_z_
Z size of the distance field.
double getSizeX() const
Gets the distance field size along the X dimension in meters.
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...
double getOriginX() const
Gets the origin (minimum value) along the X dimension.
virtual void reset()=0
Resets all points in the distance field to an uninitialize value.
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
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 ...
double getSizeY() const
Gets the distance field size along the Y dimension in meters.
virtual int getZNumCells() const =0
Gets the number of cells along the Z axis.
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,...
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...
Vec3fX< details::Vec3Data< double > > Vector3d
virtual int getXNumCells() const =0
Gets the number of cells along the X axis.
moveit_core
Author(s): Ioan Sucan
, Sachin Chitta , Acorn Pooley
autogenerated on Tue Dec 24 2024 03:27:14