37 #ifndef MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H 38 #define MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H 45 #include <visualization_msgs/Marker.h> 46 #include <visualization_msgs/MarkerArray.h> 48 #include <Eigen/Geometry> 192 void addShapeToField(
const shapes::Shape* shape,
const Eigen::Affine3d& pose);
230 void moveShapeInField(
const shapes::Shape* shape,
const Eigen::Affine3d& old_pose,
const Eigen::Affine3d& new_pose);
234 const geometry_msgs::Pose& new_pose);
245 void removeShapeFromField(
const shapes::Shape* shape,
const Eigen::Affine3d& pose);
254 virtual void reset() = 0;
275 virtual double getDistance(
double x,
double y,
double z)
const = 0;
324 double getDistanceGradient(
double x,
double y,
double z,
double& gradient_x,
double& gradient_y,
double& gradient_z,
325 bool& in_bounds)
const;
337 virtual double getDistance(
int x,
int y,
int z)
const = 0;
349 virtual bool isCellValid(
int x,
int y,
int z)
const = 0;
357 virtual int getXNumCells()
const = 0;
365 virtual int getYNumCells()
const = 0;
373 virtual int getZNumCells()
const = 0;
388 virtual bool gridToWorld(
int x,
int y,
int z,
double& world_x,
double& world_y,
double& world_z)
const = 0;
406 virtual bool worldToGrid(
double world_x,
double world_y,
double world_z,
int& x,
int& y,
int& z)
const = 0;
415 virtual bool writeToStream(std::ostream& stream)
const = 0;
426 virtual bool readFromStream(std::istream& stream) = 0;
441 void getIsoSurfaceMarkers(
double min_distance,
double max_distance,
const std::string& frame_id,
442 const ros::Time stamp, visualization_msgs::Marker& marker)
const;
457 void getGradientMarkers(
double min_radius,
double max_radius,
const std::string& frame_id,
const ros::Time& stamp,
458 visualization_msgs::MarkerArray& marker_array)
const;
486 const Eigen::Vector3d& origin,
const std::string& frame_id,
const ros::Time stamp,
487 visualization_msgs::Marker& marker)
const;
504 void getProjectionPlanes(
const std::string& frame_id,
const ros::Time& stamp,
double max_distance,
505 visualization_msgs::Marker& marker)
const;
590 virtual double getUninitializedDistance()
const = 0;
615 void setPoint(
int xCell,
int yCell,
int zCell,
double dist, geometry_msgs::Point& point, std_msgs::ColorRGBA& color,
616 double max_distance)
const;
630 #endif // MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H
PlaneVisualizationType
The plane to visualize.
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
static const double width
double size_x_
X size of the distance field.
#define MOVEIT_DEPRECATED
static const double origin_y
double getResolution() const
Gets the resolution of the distance field in meters.
double getOriginY() const
Gets the origin (minimum value) along the Y dimension.
double origin_y_
Y origin of the distance field.
double getSizeY() const
Gets the distance field size along the Y dimension in meters.
double getSizeX() const
Gets the distance field size along the X dimension in meters.
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points...
MOVEIT_CLASS_FORWARD(Shape)
double size_z_
Z size of the distance field.
static const double resolution
static const double origin_z
double origin_z_
Z origin of the distance field.
Namespace for holding classes that generate distance fields.
double origin_x_
X origin of the distance field.
double getSizeZ() const
Gets the distance field size along the Z dimension in meters.
static const double height
double getOriginZ() const
Gets the origin (minimum value) along the Z dimension.
double resolution_
Resolution of the distance field.
double size_y_
Y size of the distance field.
double getOriginX() const
Gets the origin (minimum value) along the X dimension.
int inv_twice_resolution_
Computed value 1.0/(2.0*resolution_)
static const double origin_x