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 
43 #include <vector>
44 #include <list>
45 #include <visualization_msgs/Marker.h>
46 #include <visualization_msgs/MarkerArray.h>
47 #include <Eigen/Core>
48 #include <Eigen/Geometry>
51 
52 namespace shapes
53 {
55 }
56 namespace octomap
57 {
58 class OcTree;
59 }
60 
68 namespace distance_field
69 {
72 {
76 };
77 
79 
96 {
97 public:
110  DistanceField(double size_x, double size_y, double size_z, double resolution, double origin_x, double origin_y,
111  double origin_z);
112 
113  virtual ~DistanceField();
114 
122  virtual void addPointsToField(const EigenSTL::vector_Vector3d& points) = 0;
123 
137  virtual void removePointsFromField(const EigenSTL::vector_Vector3d& points) = 0;
138 
162  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
163  const EigenSTL::vector_Vector3d& new_points) = 0;
164 
172  bool getShapePoints(const shapes::Shape* shape, const Eigen::Affine3d& pose, EigenSTL::vector_Vector3d* points);
173 
192  void addShapeToField(const shapes::Shape* shape, const Eigen::Affine3d& pose);
193 
194  // DEPRECATED form
195  MOVEIT_DEPRECATED void addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose);
196 
210  void addOcTreeToField(const octomap::OcTree* octree);
211 
230  void moveShapeInField(const shapes::Shape* shape, const Eigen::Affine3d& old_pose, const Eigen::Affine3d& new_pose);
231 
232  // DEPRECATED form
233  MOVEIT_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::Affine3d& pose);
246 
247  // DEPRECATED form
248  MOVEIT_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.
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
Definition: deprecation.h:48
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 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 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 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 x
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


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Oct 18 2020 13:16:33