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-2019, 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/random.h>
16 #include <mrpt/ros1bridge/map.h>
17 #include <mrpt/ros1bridge/pose.h>
19 #include <mrpt/system/filesystem.h> // for fileExists()
20 #include <mrpt/system/string_utils.h> // for lowerCase()
21 #include <mrpt/version.h>
22 #include <nav_msgs/OccupancyGrid.h>
23 #include <ros/console.h>
24 
25 using namespace mrpt::config;
26 using namespace mrpt::io;
31 
32 #ifndef INT8_MAX // avoid duplicated #define's
33 #define INT8_MAX 0x7f
34 #define INT8_MIN (-INT8_MAX - 1)
35 #define INT16_MAX 0x7fff
36 #define INT16_MIN (-INT16_MAX - 1)
37 #endif // INT8_MAX
38 
39 namespace mrpt::ros1bridge
40 {
41 MapHdl::MapHdl()
42 {
43  // MRPT -> ROS LUT:
45 
46 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
47  const int i_min = INT8_MIN, i_max = INT8_MAX;
48 #else
49  const int i_min = INT16_MIN, i_max = INT16_MAX;
50 #endif
51 
52  for (int i = i_min; i <= i_max; i++)
53  {
54  int8_t ros_val;
55  if (i == 0)
56  {
57  // Unknown cell (no evidence data):
58  ros_val = -1;
59  }
60  else
61  {
62  float p = 1.0 - table.l2p(i);
63  ros_val = round(p * 100.);
64  // printf("- cell -> ros = %4i -> %4i, p=%4.3f\n", i, ros_val, p);
65  }
66 
67  lut_cellmrpt2ros[static_cast<int>(i) - i_min] = ros_val;
68  }
69 
70  // ROS -> MRPT: [0,100] ->
71  for (int i = 0; i <= 100; i++)
72  {
73  const float p = 1.0 - (i / 100.0);
74  lut_cellros2mrpt[i] = table.p2l(p);
75 
76  // printf("- ros->cell=%4i->%4i p=%4.3f\n",i, lut_cellros2mrpt[i], p);
77  }
78 }
79 MapHdl* MapHdl::instance()
80 {
81  static MapHdl m; // singeleton instance
82  return &m;
83 }
84 
85 bool fromROS(const nav_msgs::OccupancyGrid& src, COccupancyGridMap2D& des)
86 {
88  if ((src.info.origin.orientation.x != 0) ||
89  (src.info.origin.orientation.y != 0) ||
90  (src.info.origin.orientation.z != 0) ||
91  (src.info.origin.orientation.w != 1))
92  {
93  std::cerr << "[fromROS] Rotated maps are not supported!\n";
94  return false;
95  }
96  float xmin = src.info.origin.position.x;
97  float ymin = src.info.origin.position.y;
98  float xmax = xmin + src.info.width * src.info.resolution;
99  float ymax = ymin + src.info.height * src.info.resolution;
100 
101  des.setSize(xmin, xmax, ymin, ymax, src.info.resolution);
102  auto inst = MapHdl::instance();
103  for (unsigned int h = 0; h < src.info.height; h++)
104  {
105  COccupancyGridMap2D::cellType* pDes = des.getRow(h);
106  const int8_t* pSrc = &src.data[h * src.info.width];
107  for (unsigned int w = 0; w < src.info.width; w++)
108  *pDes++ = inst->cellRos2Mrpt(*pSrc++);
109  }
110  return true;
111  MRPT_END
112 }
113 bool toROS(
114  const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des,
115  const std_msgs::Header& header)
116 {
117  des.header = header;
118  return toROS(src, des);
119 }
120 
121 bool toROS(const COccupancyGridMap2D& src, nav_msgs::OccupancyGrid& des)
122 {
123  des.info.width = src.getSizeX();
124  des.info.height = src.getSizeY();
125  des.info.resolution = src.getResolution();
126 
127  des.info.origin.position.x = src.getXMin();
128  des.info.origin.position.y = src.getYMin();
129  des.info.origin.position.z = 0;
130 
131  des.info.origin.orientation.x = 0;
132  des.info.origin.orientation.y = 0;
133  des.info.origin.orientation.z = 0;
134  des.info.origin.orientation.w = 1;
135 
136  // I hope the data is always aligned
137  des.data.resize(des.info.width * des.info.height);
138  for (unsigned int h = 0; h < des.info.height; h++)
139  {
140  const COccupancyGridMap2D::cellType* pSrc = src.getRow(h);
141  int8_t* pDes = &des.data[h * des.info.width];
142  for (unsigned int w = 0; w < des.info.width; w++)
143  {
144  *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
145  }
146  }
147  return true;
148 }
149 
150 bool MapHdl::loadMap(
151  CMultiMetricMap& _metric_map, const CConfigFileBase& _config_file,
152  const std::string& _map_file, const std::string& _section_name, bool _debug)
153 {
154  using namespace mrpt::maps;
155 
156  TSetOfMetricMapInitializers mapInitializers;
157  mapInitializers.loadFromConfigFile(_config_file, _section_name);
158 
159  CSimpleMap simpleMap;
160 
161  // Load the set of metric maps to consider in the experiments:
162  _metric_map.setListOfMaps(mapInitializers);
163  if (_debug) mapInitializers.dumpToConsole();
164 
165 #if MRPT_VERSION >= 0x199
167 #else
169 #endif
170  r.randomize();
171 
172  if (_debug)
173  printf(
174  "%s, _map_file.size() = %zu\n", _map_file.c_str(),
175  _map_file.size());
176  // Load the map (if any):
177  if (_map_file.size() < 3)
178  {
179  if (_debug) printf("No mrpt map file!\n");
180  return false;
181  }
182  else
183  {
184  ASSERT_(mrpt::system::fileExists(_map_file));
185 
186  // Detect file extension:
187  std::string mapExt =
189  _map_file, true)); // Ignore possible .gz extensions
190 
191  if (!mapExt.compare("simplemap"))
192  {
193  // It's a ".simplemap":
194  if (_debug) printf("Loading '.simplemap' file...");
195  CFileGZInputStream f(_map_file);
196 #if MRPT_VERSION >= 0x199
197  mrpt::serialization::archiveFrom(f) >> simpleMap;
198 #else
199  f >> simpleMap;
200 #endif
201  printf("Ok\n");
202 
203  ASSERTMSG_(
204  simpleMap.size() > 0,
205  "Simplemap was aparently loaded OK, but it is empty!");
206 
207  // Build metric map:
208  if (_debug) printf("Building metric map(s) from '.simplemap'...");
209  _metric_map.loadFromSimpleMap(simpleMap);
210  if (_debug) printf("Ok\n");
211  }
212  else if (!mapExt.compare("gridmap"))
213  {
214  // It's a ".gridmap":
215  if (_debug) printf("Loading gridmap from '.gridmap'...");
216  ASSERTMSG_(
217 #if MRPT_VERSION >= 0x199
218  _metric_map.countMapsByClass<COccupancyGridMap2D>() == 1,
219 #else
220  _metric_map.m_gridMaps.size() == 1,
221 #endif
222  "Error: Trying to load a gridmap into a multi-metric map "
223  "requires 1 gridmap member.");
224  CFileGZInputStream fm(_map_file);
225 #if MRPT_VERSION >= 0x199
227  (*_metric_map.mapByClass<COccupancyGridMap2D>());
228 #else
229  fm >> (*_metric_map.m_gridMaps[0]);
230 #endif
231  if (_debug) printf("Ok\n");
232  }
233  else
234  {
236  "Map file has unknown extension: '%s'", mapExt.c_str()));
237  return false;
238  }
239  }
240  return true;
241 }
242 
243 } // namespace mrpt::ros1bridge
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
#define MRPT_START
Definition: exceptions.h:241
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
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
GLenum GLsizei GLenum GLenum const GLvoid * table
Definition: glext.h:3535
signed char int8_t
Definition: rptypes.h:43
GLuint src
Definition: glext.h:7397
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
#define INT16_MAX
Definition: map.cpp:35
GLubyte GLubyte GLubyte GLubyte w
Definition: glext.h:4199
static CRandomGenerator randomGenerator
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:587
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
#define INT8_MAX
Definition: map.cpp:33
This class allows loading and storing values and vectors of different types from a configuration text...
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
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.
#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
GLsizei const GLchar ** string
Definition: glext.h:4116
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
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
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...
T::Ptr mapByClass(const 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 ...
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
Definition: format.cpp:16
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:36
#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:34
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.
CRandomGenerator & getRandomGenerator()
A static instance of a CRandomGenerator class, for use in single-thread applications.
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:107
GLfloat GLfloat p
Definition: glext.h:6398
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:23



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 1de0e027c Sat Sep 14 16:15:22 2019 +0200 at sáb sep 14 16:20:14 CEST 2019