map.cpp
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2018, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 
10 #include "mrpt_bridge/map.h"
11 #include "mrpt_bridge/pose.h"
12 #include <nav_msgs/OccupancyGrid.h>
13 #include <ros/console.h>
14 
15 // Only include MRPT classes that are really used to avoid slow down compilation
16 #include <mrpt/random.h>
17 
18 #if MRPT_VERSION >= 0x199
19 #include <mrpt/config/CConfigFile.h>
20 #include <mrpt/io/CFileGZInputStream.h>
21 using namespace mrpt::config;
22 using namespace mrpt::io;
23 #else
24 #include <mrpt/utils/CConfigFile.h>
25 #include <mrpt/utils/CFileGZInputStream.h>
26 using namespace mrpt::utils;
27 #endif
28 
29 #include <mrpt/system/filesystem.h> // for fileExists()
30 #include <mrpt/system/string_utils.h> // for lowerCase()
31 
32 #include <mrpt/version.h>
33 #include <mrpt/maps/COccupancyGridMap2D.h>
34 #include <mrpt/maps/CMultiMetricMap.h>
35 #include <mrpt/maps/CSimpleMap.h>
36 using mrpt::maps::CLogOddsGridMapLUT;
37 using mrpt::maps::CMultiMetricMap;
38 using mrpt::maps::COccupancyGridMap2D;
39 using mrpt::maps::CSimpleMap;
40 
41 #if MRPT_VERSION >= 0x199
42 #include <mrpt/serialization/CArchive.h>
43 #endif
44 
45 #ifndef INT8_MAX // avoid duplicated #define's
46 #define INT8_MAX 0x7f
47 #define INT8_MIN (-INT8_MAX - 1)
48 #define INT16_MAX 0x7fff
49 #define INT16_MIN (-INT16_MAX - 1)
50 #endif // INT8_MAX
51 
52 namespace mrpt_bridge
53 {
54 MapHdl* MapHdl::instance_ = NULL;
55 
56 MapHdl::MapHdl()
57 {
59  CLogOddsGridMapLUT<COccupancyGridMap2D::cellType> table;
60 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
61  lut_cellmrpt2rosPtr =
62  lut_cellmrpt2ros + INT8_MAX + 1; // center the pointer
63  lut_cellros2mrptPtr =
64  lut_cellros2mrpt + INT8_MAX + 1; // center the pointer
65  for (int i = INT8_MIN; i < INT8_MAX; i++)
66  {
67 #else
68  lut_cellmrpt2rosPtr =
69  lut_cellmrpt2ros + INT16_MAX + 1; // center the pointer
70  for (int i = INT16_MIN; INT16_MIN < INT16_MAX; i++)
71  {
72 #endif
73  float p = 1.0 - table.l2p(i);
74  int idx = round(p * 100.);
75  lut_cellmrpt2rosPtr[i] = idx;
76  // printf("- cell -> ros = %4i -> %4i, p=%4.3f\n", i, idx, p);
77  }
78  for (int i = INT8_MIN; i < INT8_MAX; i++)
79  {
80  float v = i;
81  if (v > 100) v = 50;
82  if (v < 0) v = 50;
83  float p = 1.0 - (v / 100.0);
84  int idx = table.p2l(p);
85  if (i < 0)
86  lut_cellros2mrptPtr[i] = table.p2l(0.5);
87  else if (i > 100)
88  lut_cellros2mrptPtr[i] = table.p2l(0.5);
89  else
90  lut_cellros2mrptPtr[i] = idx;
91  // printf("- ros -> cell = %4i -> %4i, p=%4.3f\n", i, idx, p);
92  fflush(stdout);
93  }
94 }
95 MapHdl::~MapHdl() {}
96 MapHdl* MapHdl::instance()
97 {
98  if (instance_ == NULL) instance_ = new MapHdl();
99  return instance_;
100 }
101 
102 bool convert(const nav_msgs::OccupancyGrid& src, COccupancyGridMap2D& des)
103 {
104  if ((src.info.origin.orientation.x != 0) ||
105  (src.info.origin.orientation.y != 0) ||
106  (src.info.origin.orientation.z != 0) ||
107  (src.info.origin.orientation.w != 1))
108  {
109  std::cerr << "Rotated maps are not supported by mrpt!" << std::endl;
110  return false;
111  }
112  float xmin = src.info.origin.position.x;
113  float ymin = src.info.origin.position.y;
114  float xmax = xmin + src.info.width * src.info.resolution;
115  float ymax = ymin + src.info.height * src.info.resolution;
116 
117  MRPT_START
118  des.setSize(xmin, xmax, ymin, ymax, src.info.resolution);
119  MRPT_END
120  // printf("--------convert: %i x %i, %4.3f, %4.3f, %4.3f, %4.3f,
121  // r:%4.3f\n",des.getSizeX(), des.getSizeY(), des.getXMin(), des.getXMax(),
122  // des.getYMin(), des.getYMax(), des.getResolution());
123 
125  for (unsigned int h = 0; h < src.info.height; h++)
126  {
127  COccupancyGridMap2D::cellType* pDes = des.getRow(h);
128  const int8_t* pSrc = &src.data[h * src.info.width];
129  for (unsigned int w = 0; w < src.info.width; w++)
130  {
131  *pDes++ = MapHdl::instance()->cellRos2Mrpt(*pSrc++);
132  }
133  }
134  return true;
135 }
136 bool convert(
137  const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des,
138  const std_msgs::Header& header)
139 {
140  des.header = header;
141  return convert(src, des);
142 }
143 
144 bool convert(const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des)
145 {
146  // printf("--------mrpt2ros: %f, %f, %f, %f, r:%f\n",src.getXMin(),
147  // src.getXMax(), src.getYMin(), src.getYMax(), src.getResolution());
148  des.info.width = src.getSizeX();
149  des.info.height = src.getSizeY();
150  des.info.resolution = src.getResolution();
151 
152  des.info.origin.position.x = src.getXMin();
153  des.info.origin.position.y = src.getYMin();
154  des.info.origin.position.z = 0;
155 
156  des.info.origin.orientation.x = 0;
157  des.info.origin.orientation.y = 0;
158  des.info.origin.orientation.z = 0;
159  des.info.origin.orientation.w = 1;
160 
162  des.data.resize(des.info.width * des.info.height);
163  for (unsigned int h = 0; h < des.info.height; h++)
164  {
165  const COccupancyGridMap2D::cellType* pSrc = src.getRow(h);
166  int8_t* pDes = &des.data[h * des.info.width];
167  for (unsigned int w = 0; w < des.info.width; w++)
168  {
169  *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
170  }
171  }
172  return true;
173 }
174 
175 const bool MapHdl::loadMap(
176  CMultiMetricMap& _metric_map, const CConfigFile& _config_file,
177  const std::string& _map_file, const std::string& _section_name, bool _debug)
178 {
179  using namespace mrpt::maps;
180 
181  TSetOfMetricMapInitializers mapInitializers;
182  mapInitializers.loadFromConfigFile(_config_file, _section_name);
183 
184  CSimpleMap simpleMap;
185 
186  // Load the set of metric maps to consider in the experiments:
187  _metric_map.setListOfMaps(mapInitializers);
188  if (_debug) mapInitializers.dumpToConsole();
189 
190 #if MRPT_VERSION >= 0x199
191  auto& r = mrpt::random::getRandomGenerator();
192 #else
193  auto& r = mrpt::random::randomGenerator;
194 #endif
195  r.randomize();
196 
197  if (_debug)
198  printf(
199  "%s, _map_file.size() = %zu\n", _map_file.c_str(),
200  _map_file.size());
201  // Load the map (if any):
202  if (_map_file.size() < 3)
203  {
204  if (_debug) printf("No mrpt map file!\n");
205  return false;
206  }
207  else
208  {
209  ASSERT_(mrpt::system::fileExists(_map_file));
210 
211  // Detect file extension:
212  std::string mapExt =
213  mrpt::system::lowerCase(mrpt::system::extractFileExtension(
214  _map_file, true)); // Ignore possible .gz extensions
215 
216  if (!mapExt.compare("simplemap"))
217  {
218  // It's a ".simplemap":
219  if (_debug) printf("Loading '.simplemap' file...");
220  CFileGZInputStream f(_map_file);
221 #if MRPT_VERSION >= 0x199
222  mrpt::serialization::archiveFrom(f) >> simpleMap;
223 #else
224  f >> simpleMap;
225 #endif
226  printf("Ok\n");
227 
228  ASSERTMSG_(
229  simpleMap.size() > 0,
230  "Simplemap was aparently loaded OK, but it is empty!");
231 
232  // Build metric map:
233  if (_debug) printf("Building metric map(s) from '.simplemap'...");
234  _metric_map.loadFromSimpleMap(simpleMap);
235  if (_debug) printf("Ok\n");
236  }
237  else if (!mapExt.compare("gridmap"))
238  {
239  // It's a ".gridmap":
240  if (_debug) printf("Loading gridmap from '.gridmap'...");
241  ASSERTMSG_(
242 #if MRPT_VERSION >= 0x199
243  _metric_map.countMapsByClass<COccupancyGridMap2D>() == 1,
244 #else
245  _metric_map.m_gridMaps.size() == 1,
246 #endif
247  "Error: Trying to load a gridmap into a multi-metric map "
248  "requires 1 gridmap member.");
249  CFileGZInputStream fm(_map_file);
250 #if MRPT_VERSION >= 0x199
251  mrpt::serialization::archiveFrom(fm) >>
252  (*_metric_map.mapByClass<COccupancyGridMap2D>());
253 #else
254  fm >> (*_metric_map.m_gridMaps[0]);
255 #endif
256  if (_debug) printf("Ok\n");
257  }
258  else
259  {
260  THROW_EXCEPTION(mrpt::format(
261  "Map file has unknown extension: '%s'", mapExt.c_str()));
262  return false;
263  }
264  }
265  return true;
266 }
267 
268 } // namespace mrpt_bridge
INT16_MAX
#define INT16_MAX
Definition: map.cpp:48
pose.h
INT16_MIN
#define INT16_MIN
Definition: map.cpp:49
mrpt_bridge::MapHdl
the map class is implemented as singeleton use map::instance ()->ros2mrpt ...
Definition: map.h:78
f
f
console.h
map.h
std_msgs::Header_
Definition: map.h:24
INT8_MAX
#define INT8_MAX
Definition: map.cpp:46
mrpt_bridge::convert
bool convert(const mrpt_msgs::ObservationRangeBeacon &_msg, const mrpt::poses::CPose3D &_pose, mrpt::obs::CObservationBeaconRanges &_obj)
nav_msgs::OccupancyGrid_
Definition: map.h:31
mrpt::utils
Definition: map.h:52
INT8_MIN
#define INT8_MIN
Definition: map.cpp:47
mrpt_bridge
File includes methods for converting CNetworkOfPoses*DInf <=> NetworkOfPoses message types.
Definition: include/mrpt_bridge/beacon.h:52
mrpt::maps
Definition: map.h:38
header
const std::string header


mrpt_bridge
Author(s): Markus Bader , Raphael Zack
autogenerated on Sun Mar 6 2022 03:48:10