cost_map.hpp

Go to the documentation of this file.

#pragma once

#include <vector>
#include <unordered_map>

// Eigen
#include <Eigen/Core>
#include <grid_map_core/BufferRegion.hpp>
#include <grid_map_core/TypeDefs.hpp>
#include "common.hpp"
#include "submap_geometry.hpp"

namespace cost_map {

class SubmapGeometry;

class CostMap
{
public:
  // type traits for use with template methods/classes using GridMap as a template parameter
  typedef cost_map::Matrix::Scalar DataType; 
  typedef cost_map::Matrix Matrix;           
  CostMap(const std::vector<std::string>& layers);

  CostMap();

  virtual ~CostMap();

  void setGeometry(const Length& length, const double resolution,
                   const Position& position = Position::Zero());

  void setGeometry(const SubmapGeometry& geometry);

  void setPosition(const Position& position) { position_ = position; }

  void add(const std::string& layer, const DataType value = NO_INFORMATION);

  void add(const std::string& layer, const Matrix& data);

  bool exists(const std::string& layer) const;

  const Matrix& get(const std::string& layer) const;

  Matrix& get(const std::string& layer);

  const Matrix& operator [](const std::string& layer) const;

  Matrix& operator [](const std::string& layer);

  bool erase(const std::string& layer);

  const std::vector<std::string>& getLayers() const;

  void setBasicLayers(const std::vector<std::string>& basicLayers);

  const std::vector<std::string>& getBasicLayers() const;

  bool hasSameLayers(const CostMap& other) const;

  DataType& atPosition(const std::string& layer, const Position& position);

  DataType atPosition(const std::string& layer,
                      const Position& position,
                      grid_map::InterpolationMethods interpolation_method=grid_map::InterpolationMethods::INTER_NEAREST
                      ) const;

  DataType& at(const std::string& layer, const Index& index);

  DataType at(const std::string& layer, const Index& index) const;

  bool getIndex(const Position& position, Index& index) const;

  bool getPosition(const Index& index, Position& position) const;

  bool isInside(const Position& position) const;

  bool isValid(const Index& index) const;

  bool isValid(const Index& index, const std::string& layer) const;

  bool isValid(const Index& index, const std::vector<std::string>& layers) const;

  bool getPosition3(const std::string& layer, const Index& index,
                    Position3& position) const;

  bool getVector(const std::string& layerPrefix, const Index& index,
                 Eigen::Vector3d& vector) const;

  CostMap getSubmap(const Position& position, const Length& length,
                    bool& isSuccess) const;

  CostMap getSubmap(const Position& position, const Length& length,
                    Index& indexInSubmap, bool& isSuccess) const;

  bool move(const Position& position, std::vector<BufferRegion>& newRegions);

  bool move(const Position& position);

  bool addDataFrom(const CostMap& other, bool extendMap,
                   bool overwriteData, bool copyAllLayers,
                   std::vector<std::string> layers = std::vector<std::string>());

  bool extendToInclude(const CostMap& other);

  void clear(const std::string& layer);

  void clearBasic();

  void clearAll();

  void setTimestamp(const Time timestamp);

  Time getTimestamp() const;

  void resetTimestamp();

  void setFrameId(const std::string& frameId);

  const std::string& getFrameId() const;

  const Length& getLength() const;

  const Position& getPosition() const;

  double getResolution() const;

  const Size& getSize() const;

  void setStartIndex(const Index& startIndex);

  const Index& getStartIndex() const;

 private:

  void clearCols(unsigned int index, unsigned int nCols);

  void clearRows(unsigned int index, unsigned int nRows);

  bool atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const;

  void resize(const Eigen::Array2i& bufferSize);

  std::string frameId_;

  Time timestamp_;

  std::unordered_map<std::string, Matrix> data_;

  std::vector<std::string> layers_;

  std::vector<std::string> basicLayers_;

  Length length_;

  double resolution_;

  Position position_;

  Size size_;

  Index startIndex_;
};

} /* namespace */