cost_map.hpp
Go to the documentation of this file.
1 
5 #pragma once
6 
7 #include <vector>
8 #include <unordered_map>
9 
10 // Eigen
11 #include <Eigen/Core>
14 #include "common.hpp"
15 #include "submap_geometry.hpp"
16 
17 namespace cost_map {
18 
19 class SubmapGeometry;
20 
34 class CostMap
35 {
36 public:
37  // type traits for use with template methods/classes using GridMap as a template parameter
38  typedef cost_map::Matrix::Scalar DataType;
45  CostMap(const std::vector<std::string>& layers);
46 
50  CostMap();
51 
55  virtual ~CostMap();
56 
63  void setGeometry(const Length& length, const double resolution,
64  const Position& position = Position::Zero());
65 
70  void setGeometry(const SubmapGeometry& geometry);
71 
76  void setPosition(const Position& position) { position_ = position; }
77 
83  void add(const std::string& layer, const DataType value = NO_INFORMATION);
84 
90  void add(const std::string& layer, const Matrix& data);
91 
97  bool exists(const std::string& layer) const;
98 
105  const Matrix& get(const std::string& layer) const;
106 
114  Matrix& get(const std::string& layer);
115 
122  const Matrix& operator [](const std::string& layer) const;
123 
131  Matrix& operator [](const std::string& layer);
132 
138  bool erase(const std::string& layer);
139 
144  const std::vector<std::string>& getLayers() const;
145 
152  void setBasicLayers(const std::vector<std::string>& basicLayers);
153 
158  const std::vector<std::string>& getBasicLayers() const;
159 
167  bool hasSameLayers(const CostMap& other) const;
168 
176  DataType& atPosition(const std::string& layer, const Position& position);
177 
186  DataType atPosition(const std::string& layer,
187  const Position& position,
189  ) const;
190 
198  DataType& at(const std::string& layer, const Index& index);
199 
207  DataType at(const std::string& layer, const Index& index) const;
208 
215  bool getIndex(const Position& position, Index& index) const;
216 
224  bool getPosition(const Index& index, Position& position) const;
225 
231  bool isInside(const Position& position) const;
232 
243  bool isValid(const Index& index) const;
244 
255  bool isValid(const Index& index, const std::string& layer) const;
256 
267  bool isValid(const Index& index, const std::vector<std::string>& layers) const;
268 
277  bool getPosition3(const std::string& layer, const Index& index,
278  Position3& position) const;
279 
287  bool getVector(const std::string& layerPrefix, const Index& index,
288  Eigen::Vector3d& vector) const;
289 
298  CostMap getSubmap(const Position& position, const Length& length,
299  bool& isSuccess) const;
300 
310  CostMap getSubmap(const Position& position, const Length& length,
311  Index& indexInSubmap, bool& isSuccess) const;
312 
321  bool move(const Position& position, std::vector<BufferRegion>& newRegions);
322 
330  bool move(const Position& position);
331 
341  bool addDataFrom(const CostMap& other, bool extendMap,
342  bool overwriteData, bool copyAllLayers,
343  std::vector<std::string> layers = std::vector<std::string>());
344 
350  bool extendToInclude(const CostMap& other);
351 
356  void clear(const std::string& layer);
357 
362  void clearBasic();
363 
369  void clearAll();
370 
375  void setTimestamp(const Time timestamp);
376 
381  Time getTimestamp() const;
382 
386  void resetTimestamp();
387 
392  void setFrameId(const std::string& frameId);
393 
398  const std::string& getFrameId() const;
399 
404  const Length& getLength() const;
405 
410  const Position& getPosition() const;
411 
416  double getResolution() const;
417 
422  const Size& getSize() const;
423 
429  void setStartIndex(const Index& startIndex);
430 
435  const Index& getStartIndex() const;
436 
437  private:
438 
444  void clearCols(unsigned int index, unsigned int nCols);
445 
451  void clearRows(unsigned int index, unsigned int nRows);
452 
460  bool atPositionLinearInterpolated(const std::string& layer, const Position& position, float& value) const;
461 
466  void resize(const Eigen::Array2i& bufferSize);
467 
469  std::string frameId_;
470 
473 
475  std::unordered_map<std::string, Matrix> data_;
476 
478  std::vector<std::string> layers_;
479 
483  std::vector<std::string> basicLayers_;
484 
487 
489  double resolution_;
490 
493 
496 
499 };
500 
501 } /* namespace */
Time timestamp_
Timestamp of the grid map (nanoseconds).
Definition: cost_map.hpp:472
Eigen::Matrix< unsigned char, Eigen::Dynamic, Eigen::Dynamic > Matrix
Definition: common.hpp:30
const unsigned char NO_INFORMATION
Definition: common.cpp:21
const Length & getLength() const
const Matrix & operator[](const std::string &layer) const
DataType & at(const std::string &layer, const Index &index)
bool isValid(const Index &index) const
const std::vector< std::string > & getBasicLayers() const
const Size & getSize() const
void clearCols(unsigned int index, unsigned int nCols)
double getResolution() const
void setStartIndex(const Index &startIndex)
void add(const std::string &layer, const DataType value=NO_INFORMATION)
Size size_
Size of the buffer (rows and cols of the data structure).
Definition: cost_map.hpp:495
void setPosition(const Position &position)
Definition: cost_map.hpp:76
void setFrameId(const std::string &frameId)
bool addDataFrom(const CostMap &other, bool extendMap, bool overwriteData, bool copyAllLayers, std::vector< std::string > layers=std::vector< std::string >())
void setGeometry(const Length &length, const double resolution, const Position &position=Position::Zero())
InterpolationMethods
grid_map::Size Size
Definition: common.hpp:46
bool getPosition3(const std::string &layer, const Index &index, Position3 &position) const
Index startIndex_
Circular buffer start indeces.
Definition: cost_map.hpp:498
bool hasSameLayers(const CostMap &other) const
void clear(const std::string &layer)
const std::string & getFrameId() const
void setBasicLayers(const std::vector< std::string > &basicLayers)
Time getTimestamp() const
DataType & atPosition(const std::string &layer, const Position &position)
std::unordered_map< std::string, Matrix > data_
Grid map data stored as layers of matrices.
Definition: cost_map.hpp:475
grid_map::Time Time
Definition: common.hpp:48
void setTimestamp(const Time timestamp)
bool getVector(const std::string &layerPrefix, const Index &index, Eigen::Vector3d &vector) const
const Position & getPosition() const
Length length_
Side length of the map in x- and y-direction [m].
Definition: cost_map.hpp:486
void clearRows(unsigned int index, unsigned int nRows)
bool erase(const std::string &layer)
bool move(const Position &position, std::vector< BufferRegion > &newRegions)
Position position_
Map position in the grid map frame [m].
Definition: cost_map.hpp:492
grid_map::Index Index
Definition: common.hpp:45
double resolution_
Map resolution in xy plane [m/cell].
Definition: cost_map.hpp:489
bool exists(const std::string &layer) const
CostMap getSubmap(const Position &position, const Length &length, bool &isSuccess) const
grid_map::Position Position
Definition: common.hpp:41
grid_map::Position3 Position3
Definition: common.hpp:43
const Index & getStartIndex() const
cost_map::Matrix Matrix
Definition: cost_map.hpp:39
std::string frameId_
Frame id of the grid map.
Definition: cost_map.hpp:469
const std::vector< std::string > & getLayers() const
void resize(const Eigen::Array2i &bufferSize)
bool extendToInclude(const CostMap &other)
cost_map::Matrix::Scalar DataType
Definition: cost_map.hpp:38
std::vector< std::string > basicLayers_
Definition: cost_map.hpp:483
grid_map::Length Length
Definition: common.hpp:47
bool isInside(const Position &position) const
bool getIndex(const Position &position, Index &index) const
bool atPositionLinearInterpolated(const std::string &layer, const Position &position, float &value) const
std::vector< std::string > layers_
Names of the data layers.
Definition: cost_map.hpp:478


cost_map_core
Author(s): Daniel Stonier
autogenerated on Mon Jun 10 2019 13:03:41