YARP
Yet Another Robot Platform
MapGrid2D.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * All rights reserved.
4  *
5  * This software may be modified and distributed under the terms of the
6  * BSD-3-Clause license. See the accompanying LICENSE file for details.
7  */
8 
9 #ifndef YARP_DEV_MAPGRID2D_H
10 #define YARP_DEV_MAPGRID2D_H
11 
12 #include <string>
13 
14 #include <yarp/os/Portable.h>
16 #include <yarp/sig/Image.h>
17 #include <yarp/math/Vec2D.h>
18 #include <yarp/dev/api.h>
19 #include <yarp/dev/MapGrid2DInfo.h>
20 
24 namespace yarp
25 {
26  namespace dev
27  {
28  namespace Nav2D
29  {
32  {
33  public:
36  //typedef yarp::math::Vec2D<int> XYCell;
37  //typedef yarp::math::Vec2D<double> XYWorld;
38 
39  enum map_flags
40  {
41  MAP_CELL_FREE = 0,
42  MAP_CELL_KEEP_OUT = 1,
43  MAP_CELL_TEMPORARY_OBSTACLE = 2,
44  MAP_CELL_ENLARGED_OBSTACLE = 3,
45  MAP_CELL_WALL = 4,
46  MAP_CELL_UNKNOWN = 5
47  };
48 
49  private:
50  //those two always have the same size
53 
54  double m_occupied_thresh;
55  double m_free_thresh;
56 
57  //std::vector<map_link> links_to_other_maps;
58 
59  private:
60  bool m_compressed_data_over_network;
61  public:
62  bool enable_map_compression_over_network (bool val);
63 
64  private:
65  //performs an obstacles enlargement on the specified cell.
66  void enlargeCell(XYCell cell);
67 
68  //conversion from pixel color to CellFlagData (yarp format) and viceversa
69  CellFlagData PixelToCellFlagData(const yarp::sig::PixelRgb& pixin) const;
70  yarp::sig::PixelRgb CellFlagDataToPixel(const CellFlagData& cellin) const;
71 
72  //conversion from pixel color to CellOccupancyData (occupancy grid, ros format) and viceversa
73  CellOccupancyData PixelToCellOccupancyData(const yarp::sig::PixelMono& pixin) const;
74  yarp::sig::PixelMono CellOccupancyDataToPixel(const CellOccupancyData& cellin) const;
75 
76  //internal methods to read a map from file, either in yarp or ROS format
77  bool loadMapYarpOnly(std::string yarp_img_filename);
78  bool loadMapROSOnly(std::string ros_yaml_filename);
79  bool loadROSParams(std::string ros_yaml_filename, std::string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t);
80  bool loadMapYarpAndRos(std::string yarp_img_filename, std::string ros_yaml_filename);
81  bool parseMapParameters(const yarp::os::Property& mapfile);
82 
83  public:
84  MapGrid2D();
85  virtual ~MapGrid2D();
86 
92  bool isWall(XYCell cell) const;
93 
99  bool isFree(XYCell cell) const;
100 
106  bool isNotFree(XYCell cell) const;
107 
114  bool isKeepOut(XYCell cell) const;
115 
121  bool getMapFlag(XYCell cell, map_flags& flag) const;
122 
128  bool setMapFlag(XYCell cell, map_flags flag);
129 
133  void clearMapTemporaryFlags();
134 
140  bool setOccupancyData(XYCell cell, double occupancy);
141 
147  bool getOccupancyData(XYCell cell, double& occupancy) const;
148 
149  bool setMapImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image);
150  bool getMapImage(yarp::sig::ImageOf<yarp::sig::PixelRgb>& image) const;
151  bool setOccupancyGrid(yarp::sig::ImageOf<yarp::sig::PixelMono>& image);
152  bool getOccupancyGrid(yarp::sig::ImageOf<yarp::sig::PixelMono>& image) const;
153 
159  bool setOrigin(double x, double y, double theta);
160 
165  void getOrigin(double& x, double& y, double& theta) const;
166 
172  bool setResolution(double resolution);
173 
178  void getResolution(double& resolution) const;
179 
185  bool setSize_in_meters(double x, double y);
186 
192  bool setSize_in_cells(size_t x, size_t y);
193 
198  void getSize_in_meters(double& x, double& y) const;
199 
204  void getSize_in_cells(size_t&x, size_t& y) const;
205 
210  size_t width() const;
211 
216  size_t height() const;
217 
223  bool setMapName(std::string map_name);
224 
229  std::string getMapName() const;
230 
231  //------------------------------utility functions-------------------------------
232 
238  bool crop(int left, int top, int right, int bottom);
239 
240 #if 0
246  bool isInsideMap(XYCell cell) const;
247 
253  bool isInsideMap(XYWorld world) const;
254 #endif
255 
260  bool isIdenticalTo(const MapGrid2D& otherMap) const;
261 
269  bool enlargeObstacles(double size);
270 
271  //-------------------------------file access functions-------------------------------
272 
278  bool loadFromFile(std::string map_filename);
279 
285  bool saveToFile(std::string map_filename) const;
286 
287  /*
288  * Read vector from a connection.
289  * return true iff a vector was read correctly
290  */
291  bool read(yarp::os::ConnectionReader& connection) override;
292 
297  bool write(yarp::os::ConnectionWriter& connection) const override;
298  };
299  }
300  }
301 }
302 
303 #endif // YARP_DEV_MAPGRID2D_H
yarp::sig::PixelMono CellFlagData
Definition: MapGrid2D.h:34
yarp::sig::PixelMono CellOccupancyData
Definition: MapGrid2D.h:35
An interface for reading from a network connection.
An interface for writing to a network connection.
This is a base class for objects that can be both read from and be written to the YARP network.
Definition: Portable.h:29
A class for storing options and configuration information.
Definition: Property.h:37
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
Definition: ImageFile.cpp:920
bool write(const ImageOf< PixelRgb > &src, const std::string &dest, image_fileformat format=FORMAT_PPM)
Definition: ImageFile.cpp:1096
unsigned char PixelMono
Monochrome pixel type.
Definition: Image.h:450
The main, catch-all namespace for YARP.
Definition: dirs.h:19
Packed RGB pixel type.
Definition: Image.h:467
#define YARP_dev_API
Definition: api.h:21