SimpleMap.hpp

Go to the documentation of this file.

// Copyright 2025 Intelligent Robotics Lab
//
// This file is part of the project Easy Navigation (EasyNav in short)
// licensed under the GNU General Public License v3.0.
// See <http://www.gnu.org/licenses/> for details.
//
// Easy Navigation program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

 
#ifndef EASYNAV_PLANNER__SIMPLEMAP_HPP_
#define EASYNAV_PLANNER__SIMPLEMAP_HPP_
 
#include <vector>
#include <stdexcept>
#include <algorithm>
#include <utility>
#include <fstream>
#include <sstream>
 
#include "nav_msgs/msg/occupancy_grid.hpp"
 
 
namespace easynav
{

class SimpleMap
{
public:
  SimpleMap();

  void initialize(
    int width, int height, double resolution, double origin_x,
    double origin_y, bool initial_value = false);

  size_t width() const {return width_;}

  size_t height() const {return height_;}

  double resolution() const {return resolution_;}

  double origin_x() const {return origin_x_;}

  double origin_y() const {return origin_y_;}

  uint8_t at(int x, int y) const;

  uint8_t & at(int x, int y);

  void fill(uint8_t value);

  void deep_copy(const SimpleMap & other);

  bool check_bounds_metric(double mx, double my) const;

  std::pair<double, double> cell_to_metric(int x, int y) const;

  std::pair<int, int> metric_to_cell(double mx, double my) const;

  void to_occupancy_grid(nav_msgs::msg::OccupancyGrid & grid_msg) const;

  void from_occupancy_grid(const nav_msgs::msg::OccupancyGrid & grid_msg);

  bool save_to_file(const std::string & path) const;

  bool load_from_file(const std::string & path);

  void print(bool view_data = false) const;

  std::shared_ptr<SimpleMap> downsample_factor(int factor) const;

  std::shared_ptr<SimpleMap> downsample(double new_resolution) const;
 
private:
  size_t width_;
  size_t height_;
  double resolution_;
  double origin_x_;
  double origin_y_;
  std::vector<uint8_t> data_;
 
  int index(int x, int y) const;
  void check_bounds(size_t x, size_t y) const;
};
 
}  // namespace easynav
 
#endif  // EASYNAV_PLANNER__SIMPLEMAP_HPP_