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 #ifndef MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H
38 #define MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H
39 
42 #include <visualization_msgs/Marker.h>
43 #include <visualization_msgs/MarkerArray.h>
44 #include <Eigen/Core>
45 #include <Eigen/Geometry>
48 
49 namespace shapes
50 {
51 MOVEIT_CLASS_FORWARD(Shape); // Defines ShapePtr, ConstPtr, WeakPtr... etc
52 }
53 namespace octomap
54 {
55 class OcTree;
56 }
57 
65 namespace distance_field
66 {
69 {
73 };
74 
75 MOVEIT_CLASS_FORWARD(DistanceField); // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc
76 
93 {
94 public:
107  DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
108  double origin_z);
109 
110  virtual ~DistanceField();
111 
119  virtual void addPointsToField(const EigenSTL::vector_Vector3d& points) = 0;
120 
134  virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points) = 0;
135 
159  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
160  const EigenSTL::vector_Vector3d& new_points) = 0;
161 
169  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points);
170 
189  void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
190 
191  // DEPRECATED form
192  [[deprecated]] void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
193 
207  void addOcTreeToField(const octomap::OcTree* octree);
208 
227  void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose,
228  const Eigen::Isometry3d& new_pose);
229 
230  // DEPRECATED form
231  [[deprecated]] void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
232  const geometry_msgs::Pose& new_pose);
233 
243  void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
244 
245  // DEPRECATED form
246  [[deprecated]] void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
247 
252  virtual void reset() = 0;
253 
273  virtual double getDistance(double x, double y, double z) const = 0;
274 
322  double getDistanceGradient(double x, double y, double z, double& gradient_x, double& gradient_y, double& gradient_z,
323  bool& in_bounds) const;
335  virtual double getDistance(int x, int y, int z) const = 0;
336 
347  virtual bool isCellValid(int x, int y, int z) const = 0;
348 
355  virtual int getXNumCells() const = 0;
356 
363  virtual int getYNumCells() const = 0;
364 
371  virtual int getZNumCells() const = 0;
372 
386  virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const = 0;
387 
404  virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const = 0;
405 
413  virtual bool writeToStream(std::ostream& stream) const = 0;
414 
424  virtual bool readFromStream(std::istream& stream) = 0;
425 
439  void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
440  const ros::Time stamp, visualization_msgs::Marker& marker) const;
441 
455  void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const ros::Time& stamp,
456  visualization_msgs::MarkerArray& marker_array) const;
457 
483  void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
484  const Eigen::Vector3d& origin, const std::string& frame_id, const ros::Time stamp,
485  visualization_msgs::Marker& marker) const;
502  void getProjectionPlanes(const std::string& frame_id, const ros::Time& stamp, double max_distance,
503  visualization_msgs::Marker& marker) const;
504 
511  double getSizeX() const
512  {
513  return size_x_;
514  }
515 
522  double getSizeY() const
523  {
524  return size_y_;
525  }
526 
533  double getSizeZ() const
534  {
535  return size_z_;
536  }
537 
544  double getOriginX() const
545  {
546  return origin_x_;
547  }
548 
555  double getOriginY() const
556  {
557  return origin_y_;
558  }
559 
566  double getOriginZ() const
567  {
568  return origin_z_;
569  }
570 
577  double getResolution() const
578  {
579  return resolution_;
580  }
581 
588  virtual double getUninitializedDistance() const = 0;
589 
590 protected:
596  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points);
597 
613  void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point, std_msgs::ColorRGBA& color,
614  double max_distance) const;
615 
616  double size_x_;
617  double size_y_;
618  double size_z_;
619  double origin_x_;
620  double origin_y_;
621  double origin_z_;
622  double resolution_;
624 };
625 
626 } // namespace distance_field
627 
628 #endif // MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_H
PlaneVisualizationType
The plane to visualize.
double getOriginX() const
Gets the origin (minimum value) along the X dimension.
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:90
std::vector< Eigen::Vector3d, Eigen::aligned_allocator< Eigen::Vector3d > > vector_Vector3d
double getOriginZ() const
Gets the origin (minimum value) along the Z dimension.
double size_x_
X size of the distance field.
double getOriginY() const
Gets the origin (minimum value) along the Y dimension.
double y
double origin_y_
Y origin of the distance field.
double getSizeY() const
Gets the distance field size along the Y dimension in meters.
double getSizeZ() const
Gets the distance field size along the Z dimension in meters.
DistanceField is an abstract base class for computing distances from sets of 3D obstacle points...
TF2SIMD_FORCE_INLINE tf2Scalar length(const Quaternion &q)
MOVEIT_CLASS_FORWARD(Shape)
double size_z_
Z size of the distance field.
double z
double origin_z_
Z origin of the distance field.
double getResolution() const
Gets the resolution of the distance field in meters.
Namespace for holding classes that generate distance fields.
double origin_x_
X origin of the distance field.
double resolution_
Resolution of the distance field.
double size_y_
Y size of the distance field.
double x
int inv_twice_resolution_
Computed value 1.0/(2.0*resolution_)
double getSizeX() const
Gets the distance field size along the X dimension in meters.


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Tue Mar 2 2021 03:51:22