distance_field.hpp

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 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 */
 
#pragma once
 
#include <moveit/macros/class_forward.hpp>
#include <moveit/distance_field/voxel_grid.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <eigen_stl_containers/eigen_stl_containers.h>
#include <moveit/macros/class_forward.hpp>
#include <rclcpp/rclcpp.hpp>
 
namespace shapes
{
MOVEIT_CLASS_FORWARD(Shape);  // Defines ShapePtr, ConstPtr, WeakPtr... etc
}
namespace octomap
{
class OcTree;
}
 
namespace distance_field
{
enum PlaneVisualizationType
{
  XY_PLANE,
  XZ_PLANE,
  YZ_PLANE
};
 
MOVEIT_CLASS_FORWARD(DistanceField);  // Defines DistanceFieldPtr, ConstPtr, WeakPtr... etc
 
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;
 
  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose,
                      EigenSTL::vector_Vector3d* points) const;
 
  void addShapeToField(const shapes::Shape* shape, const Eigen::Isometry3d& pose);
 
  void addOcTreeToField(const octomap::OcTree* octree);
 
  void moveShapeInField(const shapes::Shape* shape, const Eigen::Isometry3d& old_pose,
                        const Eigen::Isometry3d& new_pose);
 
  void removeShapeFromField(const shapes::Shape* shape, const Eigen::Isometry3d& 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 rclcpp::Time& stamp, visualization_msgs::msg::Marker& marker) const;
 
  void getGradientMarkers(double min_radius, double max_radius, const std::string& frame_id, const rclcpp::Time& stamp,
                          visualization_msgs::msg::MarkerArray& marker_array) const;
 
  void getPlaneMarkers(PlaneVisualizationType type, double length, double width, double height,
                       const Eigen::Vector3d& origin, const std::string& frame_id, const rclcpp::Time& stamp,
                       visualization_msgs::msg::Marker& marker) const;
  void getProjectionPlanes(const std::string& frame_id, const rclcpp::Time& stamp, double max_distance,
                           visualization_msgs::msg::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 getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points) const;
 
  void setPoint(int xCell, int yCell, int zCell, double dist, geometry_msgs::msg::Point& point,
                std_msgs::msg::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_; 
};
 
}  // namespace distance_field