MRPT  1.9.9
map.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2020, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
14 #include <mrpt/maps/CSimpleMap.h>
15 #include <mrpt/ros1bridge/map.h>
16 #include <mrpt/ros1bridge/pose.h>
18 #include <mrpt/system/filesystem.h> // for fileExists()
19 #include <mrpt/system/string_utils.h> // for lowerCase()
20 #include <mrpt/version.h>
21 #include <nav_msgs/OccupancyGrid.h>
22 #include <ros/console.h>
23 
24 using namespace mrpt::config;
25 using namespace mrpt::io;
30 
31 #ifndef INT8_MAX // avoid duplicated #define's
32 #define INT8_MAX 0x7f
33 #define INT8_MIN (-INT8_MAX - 1)
34 #define INT16_MAX 0x7fff
35 #define INT16_MIN (-INT16_MAX - 1)
36 #endif // INT8_MAX
37 
38 namespace mrpt::ros1bridge
39 {
40 MapHdl::MapHdl()
41 {
42  // MRPT -> ROS LUT:
44 
45 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
46  const int i_min = INT8_MIN, i_max = INT8_MAX;
47 #else
48  const int i_min = INT16_MIN, i_max = INT16_MAX;
49 #endif
50 
51  for (int i = i_min; i <= i_max; i++)
52  {
53  int8_t ros_val;
54  if (i == 0)
55  {
56  // Unknown cell (no evidence data):
57  ros_val = -1;
58  }
59  else
60  {
61  float p = 1.0 - table.l2p(i);
62  ros_val = round(p * 100.);
63  // printf("- cell -> ros = %4i -> %4i, p=%4.3f\n", i, ros_val, p);
64  }
65 
66  lut_cellmrpt2ros[static_cast<int>(i) - i_min] = ros_val;
67  }
68 
69  // ROS -> MRPT: [0,100] ->
70  for (int i = 0; i <= 100; i++)
71  {
72  const float p = 1.0 - (i / 100.0);
73  lut_cellros2mrpt[i] = table.p2l(p);
74 
75  // printf("- ros->cell=%4i->%4i p=%4.3f\n",i, lut_cellros2mrpt[i], p);
76  }
77 }
78 MapHdl* MapHdl::instance()
79 {
80  static MapHdl m; // singeleton instance
81  return &m;
82 }
83 
84 bool fromROS(const nav_msgs::OccupancyGrid& src, COccupancyGridMap2D& des)
85 {
87  if ((src.info.origin.orientation.x != 0) ||
88  (src.info.origin.orientation.y != 0) ||
89  (src.info.origin.orientation.z != 0) ||
90  (src.info.origin.orientation.w != 1))
91  {
92  std::cerr << "[fromROS] Rotated maps are not supported!\n";
93  return false;
94  }
95  float xmin = src.info.origin.position.x;
96  float ymin = src.info.origin.position.y;
97  float xmax = xmin + src.info.width * src.info.resolution;
98  float ymax = ymin + src.info.height * src.info.resolution;
99 
100  des.setSize(xmin, xmax, ymin, ymax, src.info.resolution);
101  auto inst = MapHdl::instance();
102  for (unsigned int h = 0; h < src.info.height; h++)
103  {
104  COccupancyGridMap2D::cellType* pDes = des.getRow(h);
105  const int8_t* pSrc = &src.data[h * src.info.width];
106  for (unsigned int w = 0; w < src.info.width; w++)
107  *pDes++ = inst->cellRos2Mrpt(*pSrc++);
108  }
109  return true;
110  MRPT_END
111 }
112 bool toROS(
113  const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des,
114  const std_msgs::Header& header)
115 {
116  des.header = header;
117  return toROS(src, des);
118 }
119 
120 bool toROS(const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des)
121 {
122  des.info.width = src.getSizeX();
123  des.info.height = src.getSizeY();
124  des.info.resolution = src.getResolution();
125 
126  des.info.origin.position.x = src.getXMin();
127  des.info.origin.position.y = src.getYMin();
128  des.info.origin.position.z = 0;
129 
130  des.info.origin.orientation.x = 0;
131  des.info.origin.orientation.y = 0;
132  des.info.origin.orientation.z = 0;
133  des.info.origin.orientation.w = 1;
134 
135  // I hope the data is always aligned
136  des.data.resize(des.info.width * des.info.height);
137  for (unsigned int h = 0; h < des.info.height; h++)
138  {
139  const COccupancyGridMap2D::cellType* pSrc = src.getRow(h);
140  int8_t* pDes = &des.data[h * des.info.width];
141  for (unsigned int w = 0; w < des.info.width; w++)
142  {
143  *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
144  }
145  }
146  return true;
147 }
148 
149 bool MapHdl::loadMap(
150  CMultiMetricMap& _metric_map, const CConfigFileBase& _config_file,
151  const std::string& _map_file, const std::string& _section_name, bool _debug)
152 {
153  using namespace mrpt::maps;
154 
155  TSetOfMetricMapInitializers mapInitializers;
156  mapInitializers.loadFromConfigFile(_config_file, _section_name);
157 
158  CSimpleMap simpleMap;
159 
160  // Load the set of metric maps to consider in the experiments:
161  _metric_map.setListOfMaps(mapInitializers);
162  if (_debug) mapInitializers.dumpToConsole();
163 
164  if (_debug)
165  printf(
166  "%s, _map_file.size() = %zu\n", _map_file.c_str(),
167  _map_file.size());
168  // Load the map (if any):
169  if (_map_file.size() < 3)
170  {
171  if (_debug) printf("No mrpt map file!\n");
172  return false;
173  }
174  else
175  {
176  ASSERT_(mrpt::system::fileExists(_map_file));
177 
178  // Detect file extension:
179  std::string mapExt =
181  _map_file, true)); // Ignore possible .gz extensions
182 
183  if (!mapExt.compare("simplemap"))
184  {
185  // It's a ".simplemap":
186  if (_debug) printf("Loading '.simplemap' file...");
187  CFileGZInputStream f(_map_file);
188 #if MRPT_VERSION >= 0x199
189  mrpt::serialization::archiveFrom(f) >> simpleMap;
190 #else
191  f >> simpleMap;
192 #endif
193  printf("Ok\n");
194 
195  ASSERTMSG_(
196  simpleMap.size() > 0,
197  "Simplemap was aparently loaded OK, but it is empty!");
198 
199  // Build metric map:
200  if (_debug) printf("Building metric map(s) from '.simplemap'...");
201  _metric_map.loadFromSimpleMap(simpleMap);
202  if (_debug) printf("Ok\n");
203  }
204  else if (!mapExt.compare("gridmap"))
205  {
206  // It's a ".gridmap":
207  if (_debug) printf("Loading gridmap from '.gridmap'...");
208  ASSERTMSG_(
209 #if MRPT_VERSION >= 0x199
210  _metric_map.countMapsByClass<COccupancyGridMap2D>() == 1,
211 #else
212  _metric_map.m_gridMaps.size() == 1,
213 #endif
214  "Error: Trying to load a gridmap into a multi-metric map "
215  "requires 1 gridmap member.");
216  CFileGZInputStream fm(_map_file);
217 #if MRPT_VERSION >= 0x199
219  (*_metric_map.mapByClass<COccupancyGridMap2D>());
220 #else
221  fm >> (*_metric_map.m_gridMaps[0]);
222 #endif
223  if (_debug) printf("Ok\n");
224  }
225  else
226  {
228  "Map file has unknown extension: '%s'", mapExt.c_str()));
229  return false;
230  }
231  }
232  return true;
233 }
234 
235 } // namespace mrpt::ros1bridge
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
float getYMin() const
Returns the "y" coordinate of top side of grid map.
#define MRPT_START
Definition: exceptions.h:241
float getResolution() const
Returns the resolution of the grid map.
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:32
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
T::Ptr mapByClass(size_t ith=0) const
Returns the i&#39;th map of a given class (or of a derived class), or empty smart pointer if there is no ...
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
#define INT16_MAX
Definition: map.cpp:34
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT&#39;s CStream, std::istream, std::ostream, std::stringstream.
Definition: CArchive.h:592
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
#define INT8_MAX
Definition: map.cpp:32
This class allows loading and storing values and vectors of different types from a configuration text...
cell_t p2l(const float p)
Scales a real valued probability in [0,1] to an integer representation of: log(p)-log(1-p) in the val...
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
float getXMin() const
Returns the "x" coordinate of left side of grid map.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string &sectionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file...
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
A class for storing an occupancy grid map.
bool toROS(const mrpt::obs::CObservationGPS &obj, const std_msgs::Header &msg_header, sensor_msgs::NavSatFix &msg)
Convert mrpt::obs::CObservationGPS -> sensor_msgs/NavSatFix The user must supply the "msg_header" fie...
Definition: gps.cpp:48
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:28
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally...
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
#define INT16_MIN
Definition: map.cpp:35
#define MRPT_END
Definition: exceptions.h:245
Transparently opens a compressed "gz" file and reads uncompressed data from it.
#define INT8_MIN
Definition: map.cpp:33
void setSize(float x_min, float x_max, float y_min, float y_max, float resolution, float default_value=0.5f)
Change the size of gridmap, erasing all its previous contents.
This class stores any customizable set of metric maps.
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0...
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
!
Definition: CMetricMap.h:106
Methods to convert between ROS msgs and MRPT objects for map datatypes.
Definition: map.h:30
void setListOfMaps(const mrpt::maps::TSetOfMetricMapInitializers &init)
Sets the list of internal map according to the passed list of map initializers (current maps will be ...
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: c7a3bec24 Sun Mar 29 18:33:13 2020 +0200 at dom mar 29 18:50:38 CEST 2020