00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_
00038 #define MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_
00039
00040 #include <moveit/distance_field/voxel_grid.h>
00041 #include <vector>
00042 #include <list>
00043 #include <visualization_msgs/Marker.h>
00044 #include <visualization_msgs/MarkerArray.h>
00045 #include <Eigen/Core>
00046 #include <Eigen/Geometry>
00047 #include <eigen_stl_containers/eigen_stl_containers.h>
00048 #include <geometric_shapes/shapes.h>
00049
00057 namespace distance_field
00058 {
00059
00061 enum PlaneVisualizationType
00062 {
00063 XYPlane,
00064 XZPlane,
00065 YZPlane
00066 };
00067
00083 class DistanceField
00084 {
00085 public:
00086
00099 DistanceField(double size_x, double size_y, double size_z, double resolution,
00100 double origin_x, double origin_y, double origin_z);
00101
00102 virtual ~DistanceField();
00103
00111 virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)=0;
00112
00126 virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)=0;
00127
00151 virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
00152 const EigenSTL::vector_Vector3d& new_points) = 0;
00153
00172 void addShapeToField(const shapes::Shape* shape,
00173 const geometry_msgs::Pose& pose);
00174
00188 void addOcTreeToField(const octomap::OcTree* octree);
00189
00208 void moveShapeInField(const shapes::Shape* shape,
00209 const geometry_msgs::Pose& old_pose,
00210 const geometry_msgs::Pose& new_pose);
00211
00221 void removeShapeFromField(const shapes::Shape* shape,
00222 const geometry_msgs::Pose& pose);
00223
00228 virtual void reset()=0;
00229
00249 virtual double getDistance(double x, double y, double z) const = 0;
00250
00298 double getDistanceGradient(double x, double y, double z,
00299 double& gradient_x, double& gradient_y, double& gradient_z,
00300 bool& in_bounds) const;
00312 virtual double getDistance(int x, int y, int z) const = 0;
00313
00324 virtual bool isCellValid(int x, int y, int z) const = 0;
00325
00332 virtual int getXNumCells() const = 0;
00333
00340 virtual int getYNumCells() const = 0;
00341
00348 virtual int getZNumCells() const = 0;
00349
00362 virtual bool gridToWorld(int x, int y, int z,
00363 double& world_x, double& world_y, double& world_z) const = 0;
00364
00381 virtual bool worldToGrid(double world_x, double world_y, double world_z,
00382 int& x, int& y, int& z) const = 0;
00383
00391 virtual bool writeToStream(std::ostream& stream) const = 0;
00392
00402 virtual bool readFromStream(std::istream& stream) = 0;
00403
00417 void getIsoSurfaceMarkers(double min_distance,
00418 double max_distance,
00419 const std::string &frame_id,
00420 const ros::Time stamp,
00421 visualization_msgs::Marker& marker ) const;
00422
00423
00424
00438 void getGradientMarkers(double min_radius,
00439 double max_radius,
00440 const std::string& frame_id,
00441 const ros::Time& stamp,
00442 visualization_msgs::MarkerArray& marker_array) const;
00443
00469 void getPlaneMarkers(PlaneVisualizationType type,
00470 double length,
00471 double width,
00472 double height,
00473 const Eigen::Vector3d& origin,
00474 const std::string & frame_id,
00475 const ros::Time stamp,
00476 visualization_msgs::Marker& marker) const;
00493 void getProjectionPlanes(const std::string& frame_id,
00494 const ros::Time& stamp,
00495 double max_distance,
00496 visualization_msgs::Marker& marker) const;
00497
00504 double getSizeX() const
00505 {
00506 return size_x_;
00507 }
00508
00515 double getSizeY() const
00516 {
00517 return size_y_;
00518 }
00519
00526 double getSizeZ() const
00527 {
00528 return size_z_;
00529 }
00530
00537 double getOriginX() const
00538 {
00539 return origin_x_;
00540 }
00541
00548 double getOriginY() const
00549 {
00550 return origin_y_;
00551 }
00552
00559 double getOriginZ() const
00560 {
00561 return origin_z_;
00562 }
00563
00570 double getResolution() const {
00571 return resolution_;
00572 }
00573
00580 virtual double getUninitializedDistance() const = 0;
00581
00582 protected:
00597 void setPoint(int xCell, int yCell, int zCell,
00598 double dist,
00599 geometry_msgs::Point& point,
00600 std_msgs::ColorRGBA& color,
00601 double max_distance) const;
00602
00603 double size_x_;
00604 double size_y_;
00605 double size_z_;
00606 double origin_x_;
00607 double origin_y_;
00608 double origin_z_;
00609 double resolution_;
00610 int inv_twice_resolution_;
00611 };
00612
00613 }
00614 #endif