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 <vector>
43 #include <list>
44 #include <visualization_msgs/Marker.h>
45 #include <visualization_msgs/MarkerArray.h>
46 #include <Eigen/Core>
47 #include <Eigen/Geometry>
50 
51 namespace shapes
52 {
54 }
55 namespace octomap
56 {
57 class OcTree;
58 }
59 
67 namespace distance_field
68 {
71 {
75 };
76 
78 
95 {
96 public:
109  DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
110  double origin_z);
111 
112  virtual ~DistanceField();
113 
121  virtual void addPointsToField(const EigenSTL::vector_Vector3d& points) = 0;
122 
136  virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points) = 0;
137 
161  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
162  const EigenSTL::vector_Vector3d& new_points) = 0;
163 
171  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points);
172 
191  void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
192 
193  // DEPRECATED form
194  [[deprecated]] void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
195 
209  void addOcTreeToField(const octomap::OcTree* octree);
210 
229  void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose,
230  const Eigen::Isometry3d& new_pose);
231 
232  // DEPRECATED form
233  [[deprecated]] void moveShapeInField(const shapes::Shape* shape, const geometry_msgs::Pose& old_pose,
234  const geometry_msgs::Pose& new_pose);
235 
245  void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
246 
247  // DEPRECATED form
248  [[deprecated]] void removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
249 
254  virtual void reset() = 0;
255 
275  virtual double getDistance(double x, double y, double z) const = 0;
276 
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;
338 
349  virtual bool isCellValid(int x, int y, int z) const = 0;
350 
357  virtual int getXNumCells() const = 0;
358 
365  virtual int getYNumCells() const = 0;
366 
373  virtual int getZNumCells() const = 0;
374 
388  virtual bool gridToWorld(int x, int y, int z, double& world_x, double& world_y, double& world_z) const = 0;
389 
406  virtual bool worldToGrid(double world_x, double world_y, double world_z, int& x, int& y, int& z) const = 0;
407 
415  virtual bool writeToStream(std::ostream& stream) const = 0;
416 
426  virtual bool readFromStream(std::istream& stream) = 0;
427 
441  void getIsoSurfaceMarkers(double min_distance, double max_distance, const std::string& frame_id,
442  const ros::Time stamp, visualization_msgs::Marker& marker) const;
443 
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;
459 
485  void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
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;
506 
513  double getSizeX() const
514  {
515  return size_x_;
516  }
517 
524  double getSizeY() const
525  {
526  return size_y_;
527  }
528 
535  double getSizeZ() const
536  {
537  return size_z_;
538  }
539 
546  double getOriginX() const
547  {
548  return origin_x_;
549  }
550 
557  double getOriginY() const
558  {
559  return origin_y_;
560  }
561 
568  double getOriginZ() const
569  {
570  return origin_z_;
571  }
572 
579  double getResolution() const
580  {
581  return resolution_;
582  }
583 
590  virtual double getUninitializedDistance() const = 0;
591 
592 protected:
598  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points);
599 
615  void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::Point& point, std_msgs::ColorRGBA& color,
616  double max_distance) const;
617 
618  double size_x_;
619  double size_y_;
620  double size_z_;
621  double origin_x_;
622  double origin_y_;
623  double origin_z_;
624  double resolution_;
626 };
627 
628 } // namespace distance_field
629 
630 #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
static const double width
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.
static const double resolution
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.
static const double height
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 Mon Feb 17 2020 03:50:49