distance_field.h

Go to the documentation of this file.

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2009, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Willow Garage nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Mrinal Kalakrishnan, Ken Anderson, E. Gil Jones */

#ifndef MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_
#define MOVEIT_DISTANCE_FIELD_DISTANCE_FIELD_

#include <moveit/distance_field/voxel_grid.h>
#include <vector>
#include <list>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <moveit_msgs/CollisionMap.h>
#include <eigen_stl_containers/eigen_stl_containers.h>
#include <geometric_shapes/shapes.h>

namespace distance_field
{

enum PlaneVisualizationType
{
 XYPlane,
 XZPlane,
 YZPlane
};

class DistanceField
{
public:

  DistanceField(double size_x, double size_y, double size_z, double resolution,
                double origin_x, double origin_y, double origin_z);

  virtual ~DistanceField();

  virtual void addPointsToField(const EigenSTL::vector_Vector3d &points)=0;

  virtual void removePointsFromField(const EigenSTL::vector_Vector3d &points)=0;

  virtual void updatePointsInField(const EigenSTL::vector_Vector3d& old_points,
                                   const EigenSTL::vector_Vector3d& new_points) = 0;

  void addShapeToField(const shapes::Shape* shape,
                       const geometry_msgs::Pose& pose);

  void addOcTreeToField(const octomap::OcTree* octree);

  void moveShapeInField(const shapes::Shape* shape,
                        const geometry_msgs::Pose& old_pose,
                        const geometry_msgs::Pose& new_pose);

  void removeShapeFromField(const shapes::Shape* shape,
                            const geometry_msgs::Pose& pose);

  virtual void reset()=0;

  virtual double getDistance(double x, double y, double z) const = 0;

  double getDistanceGradient(double x, double y, double z,
                             double& gradient_x, double& gradient_y, double& gradient_z,
                             bool& in_bounds) const;
  virtual double getDistance(int x, int y, int z) const = 0;

  virtual bool isCellValid(int x, int y, int z) const = 0;

  virtual int getXNumCells() const = 0;

  virtual int getYNumCells() const = 0;

  virtual int getZNumCells() const = 0;

  virtual bool gridToWorld(int x, int y, int z,
                           double& world_x, double& world_y, double& world_z) const = 0;

  virtual bool worldToGrid(double world_x, double world_y, double world_z,
                           int& x, int& y, int& z) const = 0;

  virtual bool writeToStream(std::ostream& stream) const = 0;

  virtual bool readFromStream(std::istream& stream) = 0;

  void getIsoSurfaceMarkers(double min_distance,
                            double max_distance,
                            const std::string &frame_id,
                            const ros::Time stamp,
                            visualization_msgs::Marker& marker ) const;



  void getGradientMarkers(double min_radius,
                          double max_radius,
                          const std::string& frame_id,
                          const ros::Time& stamp,
                          visualization_msgs::MarkerArray& marker_array) const;

  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;
  void getProjectionPlanes(const std::string& frame_id,
                           const ros::Time& stamp,
                           double max_distance,
                           visualization_msgs::Marker& marker) const;

  double getSizeX() const
  {
    return size_x_;
  }

  double getSizeY() const
  {
    return size_y_;
  }

  double getSizeZ() const
  {
    return size_z_;
  }

  double getOriginX() const
  {
    return origin_x_;
  }

  double getOriginY() const
  {
    return origin_y_;
  }

  double getOriginZ() const
  {
    return origin_z_;
  }

  double getResolution() const {
    return resolution_;
  }

  virtual double getUninitializedDistance() const = 0;

protected:
  void setPoint(int xCell, int yCell, int zCell,
                double dist,
                geometry_msgs::Point& point,
                std_msgs::ColorRGBA& color,
                double max_distance) const;

  double size_x_;               
  double size_y_;               
  double size_z_;               
  double origin_x_;             
  double origin_y_;             
  double origin_z_;             
  double resolution_;           
  int inv_twice_resolution_;    
};

}
#endif