distance_field.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */
36 
37 #pragma once
38 
41 #include <visualization_msgs/Marker.h>
42 #include <visualization_msgs/MarkerArray.h>
43 #include <Eigen/Core>
44 #include <Eigen/Geometry>
45 #include <eigen_stl_containers/eigen_stl_containers.h>
47 
48 namespace shapes
49 {
50 MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc
51 }
52 namespace octomap
53 {
54 class OcTree;
55 }
56 
64 namespace distance_field
65 {
68 {
72 };
73 
74 MOVEIT_CLASS_FORWARD(DistanceField); // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc
75 
92 {
93 public:
106  DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
107  double origin_z);
108 
109  virtual ~DistanceField();
110 
118  virtual void addPointsToField(const EigenSTL::vector_Vector3d& points) = 0;
119 
133  virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points) = 0;
134 
158  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
159  const EigenSTL::vector_Vector3d& new_points) = 0;
160 
168  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points);
169 
188  void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
189 
190  // DEPRECATED form
191  [[deprecated]] void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
192 
206  void addOcTreeToField(const octomap::OcTree* octree);
207 
226  void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose,
227  const Eigen::Isometry3d& new_pose);
228 
229  // DEPRECATED form
230  [[deprecated]] void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
231  const geometry_msgs::Pose& new_pose);
232 
242  void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
243 
244  // DEPRECATED form
245  [[deprecated]] void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
246 
251  virtual void reset() = 0;
252 
272  virtual double getDistance(double x, double y, double z) const = 0;
273 
321  double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
322  bool& in_bounds) const;
334  virtual double getDistance(int x, int y, int z) const = 0;
335 
346  virtual bool isCellValid(int x, int y, int z) const = 0;
347 
354  virtual int getXNumCells() const = 0;
355 
362  virtual int getYNumCells() const = 0;
363 
370  virtual int getZNumCells() const = 0;
371 
385  virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const = 0;
386 
403  virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const = 0;
404 
412  virtual bool writeToStream(std::ostream& stream) const = 0;
413 
423  virtual bool readFromStream(std::istream& stream) = 0;
424 
438  void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
439  const ros::Time stamp, visualization_msgs::Marker& marker) const;
440 
454  void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const ros::Time& stamp,
455  visualization_msgs::MarkerArray& marker_array) const;
456 
482  void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
483  const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp,
484  visualization_msgs::Marker& marker) const;
501  void getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_distance,
502  visualization_msgs::Marker& marker) const;
503 
510  double getSizeX() const
511  {
512  return size_x_;
513  }
514 
521  double getSizeY() const
522  {
523  return size_y_;
524  }
525 
532  double getSizeZ() const
533  {
534  return size_z_;
535  }
536 
543  double getOriginX() const
544  {
545  return origin_x_;
546  }
547 
554  double getOriginY() const
555  {
556  return origin_y_;
557  }
558 
565  double getOriginZ() const
566  {
567  return origin_z_;
568  }
569 
576  double getResolution() const
577  {
578  return resolution_;
579  }
580 
587  virtual double getUninitializedDistance() const = 0;
588 
589 protected:
595  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points);
596 
612  void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point, std_msgs::ColorRGBA& color,
613  double max_distance) const;
614 
615  double size_x_;
616  double size_y_;
617  double size_z_;
618  double origin_x_;
619  double origin_y_;
620  double origin_z_;
621  double resolution_;
623 };
624 
625 } // namespace distance_field
distance_field::DistanceField::~DistanceField
virtual ~DistanceField()
distance_field::DistanceField::updatePointsInField
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...
shapes
distance_field::DistanceField::getResolution
double getResolution() const
Gets the resolution of the distance field in meters.
Definition: distance_field.h:576
distance_field::DistanceField::removePointsFromField
virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)=0
Remove a set of obstacle points from the distance field, updating distance values accordingly.
distance_field::DistanceField::origin_z_
double origin_z_
Z origin of the distance field.
Definition: distance_field.h:620
distance_field::DistanceField::getDistanceGradient
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...
Definition: distance_field.cpp:94
distance_field::DistanceField::getGradientMarkers
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 ...
Definition: distance_field.cpp:164
distance_field::DistanceField::writeToStream
virtual bool writeToStream(std::ostream &stream) const =0
Writes the contents of the distance field to the supplied stream.
distance_field::DistanceField::getIsoSurfaceMarkers
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...
Definition: distance_field.cpp:120
distance_field::XZ_PLANE
@ XZ_PLANE
Definition: distance_field.h:70
distance_field::DistanceField::worldToGrid
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...
class_forward.h
distance_field::DistanceField::origin_x_
double origin_x_
X origin of the distance field.
Definition: distance_field.h:618
shapes::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(Shape)
shapes::Shape
distance_field::XY_PLANE
@ XY_PLANE
Definition: distance_field.h:69
distance_field::MOVEIT_CLASS_FORWARD
MOVEIT_CLASS_FORWARD(DistanceField)
distance_field::DistanceField::setPoint
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.
Definition: distance_field.cpp:483
distance_field::PlaneVisualizationType
PlaneVisualizationType
The plane to visualize.
Definition: distance_field.h:67
octomap::OcTree
distance_field::DistanceField::size_y_
double size_y_
Y size of the distance field.
Definition: distance_field.h:616
distance_field::DistanceField::addShapeToField
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...
Definition: distance_field.cpp:253
distance_field::DistanceField::addOcTreeToField
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...
Definition: distance_field.cpp:314
distance_field::DistanceField
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points....
Definition: distance_field.h:91
distance_field::DistanceField::removeShapeFromField
void removeShapeFromField(const shapes::Shape *shape, const Eigen::Isometry3d &pose)
All points corresponding to the shape are removed from the distance field.
Definition: distance_field.cpp:352
y
double y
distance_field::DistanceField::getDistance
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...
distance_field::DistanceField::getShapePoints
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...
Definition: distance_field.cpp:228
distance_field::DistanceField::getOriginY
double getOriginY() const
Gets the origin (minimum value) along the Y dimension.
Definition: distance_field.h:554
distance_field::DistanceField::gridToWorld
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...
distance_field::DistanceField::getOcTreePoints
void getOcTreePoints(const octomap::OcTree *octree, EigenSTL::vector_Vector3d *points)
Get the points associated with an octree.
Definition: distance_field.cpp:268
distance_field::DistanceField::resolution_
double resolution_
Resolution of the distance field.
Definition: distance_field.h:621
distance_field::DistanceField::getYNumCells
virtual int getYNumCells() const =0
Gets the number of cells along the Y axis.
distance_field::YZ_PLANE
@ YZ_PLANE
Definition: distance_field.h:71
distance_field::DistanceField::getSizeZ
double getSizeZ() const
Gets the distance field size along the Z dimension in meters.
Definition: distance_field.h:532
distance_field::DistanceField::size_x_
double size_x_
X size of the distance field.
Definition: distance_field.h:615
distance_field
Namespace for holding classes that generate distance fields.
Definition: distance_field.h:64
point
std::chrono::system_clock::time_point point
distance_field::DistanceField::addPointsToField
virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)=0
Add a set of obstacle points to the distance field, updating distance values accordingly....
distance_field::DistanceField::origin_y_
double origin_y_
Y origin of the distance field.
Definition: distance_field.h:619
distance_field::DistanceField::getOriginZ
double getOriginZ() const
Gets the origin (minimum value) along the Z dimension.
Definition: distance_field.h:565
distance_field::DistanceField::readFromStream
virtual bool readFromStream(std::istream &stream)=0
Reads, parameterizes, and populates the distance field based on the supplied stream.
ros::Time
distance_field::DistanceField::inv_twice_resolution_
int inv_twice_resolution_
Computed value 1.0/(2.0*resolution_)
Definition: distance_field.h:622
distance_field::DistanceField::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.
Definition: distance_field.cpp:79
distance_field::DistanceField::getUninitializedDistance
virtual double getUninitializedDistance() const =0
Gets a distance value for an invalid cell.
distance_field::DistanceField::size_z_
double size_z_
Z size of the distance field.
Definition: distance_field.h:617
distance_field::DistanceField::getSizeX
double getSizeX() const
Gets the distance field size along the X dimension in meters.
Definition: distance_field.h:510
distance_field::DistanceField::getPlaneMarkers
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...
Definition: distance_field.cpp:372
distance_field::DistanceField::getOriginX
double getOriginX() const
Gets the origin (minimum value) along the X dimension.
Definition: distance_field.h:543
voxel_grid.h
distance_field::DistanceField::reset
virtual void reset()=0
Resets all points in the distance field to an uninitialize value.
x
double x
octomap
length
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
distance_field::DistanceField::moveShapeInField
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 ...
Definition: distance_field.cpp:321
distance_field::DistanceField::getSizeY
double getSizeY() const
Gets the distance field size along the Y dimension in meters.
Definition: distance_field.h:521
distance_field::DistanceField::getZNumCells
virtual int getZNumCells() const =0
Gets the number of cells along the Z axis.
distance_field::DistanceField::getProjectionPlanes
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,...
Definition: distance_field.cpp:498
z
double z
distance_field::DistanceField::isCellValid
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...
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
distance_field::DistanceField::getXNumCells
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 Oct 12 2021 02:24:44