MRPT  2.0.4
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
mrpt::config
Definition: config/CConfigFile.h:15
INT16_MAX
#define INT16_MAX
Definition: map.cpp:34
filesystem.h
mrpt::config::CLoadableOptions::dumpToConsole
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
Definition: CLoadableOptions.cpp:43
COccupancyGridMap2D.h
pose.h
mrpt::io
Definition: img/CImage.h:24
mrpt::maps::CMultiMetricMap::mapByClass
T::Ptr mapByClass(size_t ith=0) const
Returns the i'th map of a given class (or of a derived class), or empty smart pointer if there is no ...
Definition: CMultiMetricMap.h:156
string_utils.h
CMultiMetricMap.h
mrpt::maps::COccupancyGridMap2D::getRow
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
Definition: COccupancyGridMap2D.h:369
mrpt::ros1bridge::MapHdl
Methods to convert between ROS msgs and MRPT objects for map datatypes.
Definition: map.h:31
mrpt::maps::CLogOddsGridMapLUT::l2p
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
Definition: CLogOddsGridMapLUT.h:85
CConfigFile.h
INT16_MIN
#define INT16_MIN
Definition: map.cpp:35
mrpt::obs::gnss::header
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
Definition: gnss_messages_novatel.h:246
mrpt::maps::CLogOddsGridMapLUT::p2l
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...
Definition: CLogOddsGridMapLUT.h:109
THROW_EXCEPTION
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
ASSERT_
#define ASSERT_(f)
Defines an assertion mechanism.
Definition: exceptions.h:120
mrpt::system::fileExists
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
Definition: filesystem.cpp:128
mrpt::system::lowerCase
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
Definition: string_utils.cpp:26
mrpt::maps::TSetOfMetricMapInitializers
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
Definition: TMetricMapInitializer.h:90
mrpt::maps::COccupancyGridMap2D::getSizeY
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
Definition: COccupancyGridMap2D.h:276
mrpt::maps::CLogOddsGridMapLUT
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
Definition: CLogOddsGridMapLUT.h:28
mrpt::maps::COccupancyGridMap2D::setSize
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.
Definition: COccupancyGridMap2D_common.cpp:140
mrpt::io::CFileGZInputStream
Transparently opens a compressed "gz" file and reads uncompressed data from it.
Definition: io/CFileGZInputStream.h:27
mrpt::round
int round(const T value)
Returns the closer integer (int) to x.
Definition: round.h:24
MRPT_START
#define MRPT_START
Definition: exceptions.h:241
mrpt::config::CConfigFileBase
This class allows loading and storing values and vectors of different types from a configuration text...
Definition: config/CConfigFileBase.h:45
mrpt::maps::CMultiMetricMap
This class stores any customizable set of metric maps.
Definition: CMultiMetricMap.h:121
mrpt::maps::TSetOfMetricMapInitializers::loadFromConfigFile
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.
Definition: TMetricMapInitializer.cpp:69
map.h
mrpt::maps::COccupancyGridMap2D::getYMin
float getYMin() const
Returns the "y" coordinate of top side of grid map.
Definition: COccupancyGridMap2D.h:282
mrpt::system::extractFileExtension
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
Definition: filesystem.cpp:98
INT8_MAX
#define INT8_MAX
Definition: map.cpp:32
ASSERTMSG_
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
Definition: exceptions.h:108
mrpt::ros1bridge::fromROS
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
Definition: gps.cpp:20
mrpt::maps::CMultiMetricMap::setListOfMaps
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 ...
Definition: CMultiMetricMap.cpp:113
mrpt::serialization::archiveFrom
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
Definition: CArchive.h:592
mrpt::maps::COccupancyGridMap2D::getResolution
float getResolution() const
Returns the resolution of the grid map.
Definition: COccupancyGridMap2D.h:286
mrpt::maps::CMultiMetricMap::countMapsByClass
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
Definition: CMultiMetricMap.h:169
mrpt::maps::COccupancyGridMap2D::getXMin
float getXMin() const
Returns the "x" coordinate of left side of grid map.
Definition: COccupancyGridMap2D.h:278
mrpt::maps::CMetricMap::loadFromSimpleMap
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
!This is an overloaded member function, provided for convenience. It differs from the above function ...
Definition: CMetricMap.h:106
MRPT_END
#define MRPT_END
Definition: exceptions.h:245
mrpt::ros1bridge
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
Definition: gps.h:29
mrpt::maps::CSimpleMap
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
Definition: CSimpleMap.h:33
CFileGZInputStream.h
mrpt::maps::COccupancyGridMap2D
A class for storing an occupancy grid map.
Definition: COccupancyGridMap2D.h:56
INT8_MIN
#define INT8_MIN
Definition: map.cpp:33
mrpt::maps::COccupancyGridMap2D::getSizeX
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
Definition: COccupancyGridMap2D.h:274
mrpt::maps
Definition: CBeacon.h:22
CArchive.h
CSimpleMap.h
mrpt::ros1bridge::toROS
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
mrpt::format
std::string std::string format(std::string_view fmt, ARGS &&... args)
Definition: format.h:26



Page generated by Doxygen 1.8.18 for MRPT 2.0.4 at Thu Sep 24 07:14:18 UTC 2020