grid.hpp

Go to the documentation of this file.

/*********************************************************************
 * BSD 3-Clause License
 *
 * Copyright (c) 2020 Northwestern University
 * 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 copyright holder 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 HOLDER 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.
 *********************************************************************/
#ifndef GRID_HPP
#define GRID_HPP
 
#include <iosfwd>
#include <cmath>
#include <vector>
// #include <memory>
 
#include <geometry_msgs/Pose.h>
#include <nav_msgs/OccupancyGrid.h>
 
namespace ergodic_exploration
{
typedef std::vector<int8_t> GridData;
 
inline unsigned int axis_length(double lower, double upper, double resolution)
{
  return static_cast<unsigned int>(std::round((upper - lower) / resolution));
}
 
inline double axis_upper(double lower, double resolution, unsigned int size)
{
  return static_cast<double>(resolution * size) + lower;
}
 
class GridMap
{
private:
  friend class OccupancyMapper;
 
public:
  GridMap(double xmin, double xmax, double ymin, double ymax, double resolution,
          const GridData& grid_data);
 
  GridMap(const nav_msgs::OccupancyGrid::ConstPtr& grid_msg);
 
  GridMap();
 
  void update(const nav_msgs::OccupancyGrid::ConstPtr& grid_msg);
 
  bool gridBounds(unsigned int i, unsigned int j) const;
 
  bool gridBounds(unsigned int idx) const;
 
  unsigned int grid2RowMajor(unsigned int i, unsigned int j) const;
 
  std::vector<unsigned int> rowMajor2Grid(unsigned int idx) const;
 
  std::vector<double> grid2World(unsigned int i, unsigned int j) const;
 
  std::vector<double> grid2World(unsigned int idx) const;
 
  std::vector<unsigned int> world2Grid(double x, double y) const;
 
  unsigned int world2RowMajor(double x, double y) const;
 
  double getCell(double x, double y) const;
 
  double getCell(unsigned int i, unsigned int j) const;
 
  double getCell(unsigned int idx) const;
 
  void print() const;
 
  const GridData& gridData() const
  {
    return grid_data_;
  }
 
  double resolution() const
  {
    return resolution_;
  }
 
  double xmin() const
  {
    return xmin_;
  }
 
  double ymin() const
  {
    return ymin_;
  }
 
  double xmax() const
  {
    return xmax_;
  }
 
  double ymax() const
  {
    return ymax_;
  }
 
  unsigned int xsize() const
  {
    return xsize_;
  }
 
  unsigned int ysize() const
  {
    return ysize_;
  }
 
  unsigned int size() const
  {
    return xsize_ * ysize_;
  }
 
private:
  unsigned int xsize_, ysize_;
  double resolution_, xmin_, ymin_, xmax_, ymax_;
  GridData grid_data_;
};
 
}  // namespace ergodic_exploration
#endif