occupancy_grid.h

Go to the documentation of this file.

/*
 * Copyright (c) 2010, Maxim Likhachev
 * 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 University of Pennsylvania 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.
 */

#include <sys/stat.h>
#include <vector>
#include <fstream>
#include <ros/ros.h>
#include <sbpl/config.h>
#include <LinearMath/btTransform.h>
#include <distance_field/distance_field.h>
#include <distance_field/voxel_grid.h>
#include <distance_field/propagation_distance_field.h>

using namespace std;

#ifndef _OCCUPANCY_GRID_
#define _OCCUPANCY_GRID_

class OccupancyGrid{

  public:
   
    OccupancyGrid();

    OccupancyGrid(double dim_x, double dim_y, double dim_z, double resolution, double origin_x, double origin_y, double origin_z);

    ~OccupancyGrid();

    inline void gridToWorld(int x, int y, int z, double &wx, double &wy, double &wz);
    
    inline void worldToGrid(double wx, double wy, double wz, int &x, int &y, int &z); 

    inline unsigned char getCell(int x, int y, int z);

    inline double getCell(int *xyz);

    inline bool isInBounds(int x, int y, int z);

    const distance_field::PropagationDistanceField* getDistanceFieldPtr();
    
    void getGridSize(int &dim_x, int &dim_y, int &dim_z);

    void getGridSize(short unsigned int *dims); //FILL IN THIS FUNCTION

    void setWorldSize(double dim_x, double dim_y, double dim_z);
    
    void getWorldSize(double &dim_x, double &dim_y, double &dim_z);

    void setOrigin(double wx, double wy, double wz);
    
    void getOrigin(double &wx, double &wy, double &wz);

    void setResolution(double resolution_m);
    
    double getResolution();

    void updateFromCollisionMap(const mapping_msgs::CollisionMap &collision_map);
    
    void visualize();
    
    void addCollisionCuboid(double origin_x, double origin_y, double origin_z, double size_x, double size_y, double size_z);

    void addPointsToField(const std::vector<btVector3> &points);

    void getVoxelsInBox(const geometry_msgs::Pose &pose, const std::vector<double> &dim, std::vector<btVector3> &voxels);
    bool saveGridToBinaryFile(std::string filename);

    void printGridFromBinaryFile(std::string filename);

    std::string getReferenceFrame();

  private:

    double origin_[3];
    double grid_resolution_;
    double world_size_[3];
    double prop_distance_;
    int grid_size_[3];

    std::string reference_frame_;
    distance_field::PropagationDistanceField* grid_;

    std::vector<btVector3> cuboid_points_;
};

inline void OccupancyGrid::gridToWorld(int x, int y, int z, double &wx, double &wy, double &wz)
{
  grid_->gridToWorld(x, y, z, wx, wy, wz); 
}

inline void OccupancyGrid::worldToGrid(double wx, double wy, double wz, int &x, int &y, int &z)
{
  grid_->worldToGrid (wx, wy, wz, x, y, z);
}

inline unsigned char OccupancyGrid::getCell(int x, int y, int z)
{
  return (unsigned char)(grid_->getDistanceFromCell(x,y,z) / grid_resolution_);
}

inline double OccupancyGrid::getCell(int *xyz)
{
  return grid_->getDistanceFromCell(xyz[0],xyz[1],xyz[2]);
}

inline bool OccupancyGrid::isInBounds(int x, int y, int z)
{
  if(x >= grid_size_[0] || 0 > x || y >= grid_size_[1] || 0 > y || z >= grid_size_[2] || 0 > z)
    return false;
  
  return true;
}

inline std::string OccupancyGrid::getReferenceFrame()
{
  return reference_frame_;
}

inline void OccupancyGrid::addPointsToField(const std::vector<btVector3> &points)
{
  grid_->addPointsToField(points);
}

#endif

---

sbpl_arm_planner
Author(s): Benjamin Cohen/bcohen@seas.upenn.edu
autogenerated on Wed Feb 29 11:46:16 2012