Go to the documentation of this file.
20 #include <mrpt/version.h>
21 #include <nav_msgs/OccupancyGrid.h>
22 #include <ros/console.h>
31 #ifndef INT8_MAX // avoid duplicated #define's
33 #define INT8_MIN (-INT8_MAX - 1)
34 #define INT16_MAX 0x7fff
35 #define INT16_MIN (-INT16_MAX - 1)
45 #ifdef OCCUPANCY_GRIDMAP_CELL_SIZE_8BITS
51 for (
int i = i_min; i <= i_max; i++)
61 float p = 1.0 - table.
l2p(i);
62 ros_val =
round(p * 100.);
66 lut_cellmrpt2ros[
static_cast<int>(i) - i_min] = ros_val;
70 for (
int i = 0; i <= 100; i++)
72 const float p = 1.0 - (i / 100.0);
73 lut_cellros2mrpt[i] = table.
p2l(p);
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))
92 std::cerr <<
"[fromROS] Rotated maps are not supported!\n";
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;
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++)
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++);
114 const std_msgs::Header&
header)
117 return toROS(src, des);
126 des.info.origin.position.x = src.
getXMin();
127 des.info.origin.position.y = src.
getYMin();
128 des.info.origin.position.z = 0;
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;
136 des.data.resize(des.info.width * des.info.height);
137 for (
unsigned int h = 0; h < des.info.height; h++)
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++)
143 *pDes++ = MapHdl::instance()->cellMrpt2Ros(*pSrc++);
149 bool MapHdl::loadMap(
151 const std::string& _map_file,
const std::string& _section_name,
bool _debug)
166 "%s, _map_file.size() = %zu\n", _map_file.c_str(),
169 if (_map_file.size() < 3)
171 if (_debug) printf(
"No mrpt map file!\n");
183 if (!mapExt.compare(
"simplemap"))
186 if (_debug) printf(
"Loading '.simplemap' file...");
188 #if MRPT_VERSION >= 0x199
196 simpleMap.size() > 0,
197 "Simplemap was aparently loaded OK, but it is empty!");
200 if (_debug) printf(
"Building metric map(s) from '.simplemap'...");
202 if (_debug) printf(
"Ok\n");
204 else if (!mapExt.compare(
"gridmap"))
207 if (_debug) printf(
"Loading gridmap from '.gridmap'...");
209 #
if MRPT_VERSION >= 0x199
212 _metric_map.m_gridMaps.size() == 1,
214 "Error: Trying to load a gridmap into a multi-metric map "
215 "requires 1 gridmap member.");
217 #if MRPT_VERSION >= 0x199
221 fm >> (*_metric_map.m_gridMaps[0]);
223 if (_debug) printf(
"Ok\n");
228 "Map file has unknown extension: '%s'", mapExt.c_str()));
void dumpToConsole() const
Just like dumpToTextStream() but sending the text to the console (std::cout)
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 ...
cellType * getRow(int cy)
Access to a "row": mainly used for drawing grid as a bitmap efficiently, do not use it normally.
Methods to convert between ROS msgs and MRPT objects for map datatypes.
float l2p(const cell_t l)
Scales an integer representation of the log-odd into a real valued probability in [0,...
nv_oem6_header_t header
Novatel frame: NV_OEM6_BESTPOS.
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...
#define THROW_EXCEPTION(msg)
#define ASSERT_(f)
Defines an assertion mechanism.
bool fileExists(const std::string &fileName)
Test if a given file (or directory) exists.
std::string lowerCase(const std::string &str)
Returns an lower-case version of a string.
A set of TMetricMapInitializer structures, passed to the constructor CMultiMetricMap::CMultiMetricMap...
unsigned int getSizeY() const
Returns the vertical size of grid map in cells count.
One static instance of this struct should exist in any class implementing CLogOddsGridMap2D to hold t...
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.
int round(const T value)
Returns the closer integer (int) to x.
This class allows loading and storing values and vectors of different types from a configuration text...
This class stores any customizable set of metric maps.
void loadFromConfigFile(const mrpt::config::CConfigFileBase &source, const std::string §ionName) override
Loads the configuration for the set of internal maps from a textual definition in an INI-like file.
float getYMin() const
Returns the "y" coordinate of top side of grid map.
std::string extractFileExtension(const std::string &filePath, bool ignore_gz=false)
Extract the extension of a filename.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
bool fromROS(const sensor_msgs::NavSatFix &msg, mrpt::obs::CObservationGPS &obj)
Convert sensor_msgs/NavSatFix -> mrpt::obs::CObservationGPS.
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 ...
CArchiveStreamBase< STREAM > archiveFrom(STREAM &s)
Helper function to create a templatized wrapper CArchive object for a: MRPT's CStream,...
float getResolution() const
Returns the resolution of the grid map.
std::size_t countMapsByClass() const
Count how many maps exist of the given class (or derived class)
float getXMin() const
Returns the "x" coordinate of left side of grid map.
void loadFromSimpleMap(const mrpt::maps::CSimpleMap &Map)
!This is an overloaded member function, provided for convenience. It differs from the above function ...
ROS message: http://docs.ros.org/api/sensor_msgs/html/msg/NavSatFix.html MRPT message: https://github...
This class stores a sequence of <Probabilistic Pose,SensoryFrame> pairs, thus a "metric map" can be t...
A class for storing an occupancy grid map.
unsigned int getSizeX() const
Returns the horizontal size of grid map in cells count.
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...
std::string std::string format(std::string_view fmt, ARGS &&... args)
Page generated by Doxygen 1.8.17 for MRPT 2.0.4 at Sun Jul 19 17:54:30 UTC 2020 | |