6#define _USE_MATH_DEFINES
20#if defined (YARP_HAS_ZLIB)
32#define DEG2RAD M_PI/180.0
36#define RAD2DEG 180.0/M_PI
43 std::string path = p.parent_path().string();
44 if (path.empty())
return path;
46 path.push_back (std::filesystem::path::preferred_separator);
74 for (
size_t y = 0; y <
m_height; y++) {
75 for (
size_t x = 0; x <
m_width; x++) {
81 for (
size_t y = 0; y <
m_height; y++) {
82 for (
size_t x = 0; x <
m_width; x++) {
100 m_occupied_thresh = 0.80;
101 m_free_thresh = 0.20;
102 for (
size_t y = 0; y <
m_height; y++)
104 for (
size_t x = 0; x <
m_width; x++)
110#if defined (YARP_HAS_ZLIB)
111 m_compressed_data_over_network =
true;
113 m_compressed_data_over_network =
false;
189 for (
size_t y = 0; y <
m_height; y++)
191 for (
size_t x = 0; x <
m_width; x++)
193 image.safePixel(x, y) = CellFlagDataToPixel(m_map_flags.
safePixel(x, y));
204 yError() <<
"The size of given image does not correspond to the current map. Use method setSize() first.";
207 for (
size_t y = 0; y <
m_height; y++)
209 for (
size_t x = 0; x <
m_width; x++)
211 m_map_flags.
safePixel(x, y) = PixelToCellFlagData(
image.safePixel(x, y));
221 for (
size_t y = 0; y <
m_height; y++)
223 for (
size_t x = 0; x <
m_width; x++)
234 for (
size_t repeat = 0; repeat <
repeat_num; repeat++)
238 for (
size_t y = 0; y <
m_height; y++)
240 for (
size_t x = 0; x <
m_width; x++)
312 if (line.find(
"origin") != std::string::npos)
314 std::replace(line.begin(), line.end(),
',',
' ');
315 std::replace(line.begin(), line.end(),
'[',
'(');
316 std::replace(line.begin(), line.end(),
']',
')');
332 if (
bbb.check(
"image:") ==
false) {
yError() <<
"missing image"; ret =
false; }
336 if (
bbb.check(
"resolution:") ==
false) {
yError() <<
"missing resolution"; ret =
false; }
337 resolution =
bbb.find(
"resolution:").asFloat64();
339 if (
bbb.check(
"origin:") ==
false) {
yError() <<
"missing origin"; ret =
false; }
340 Bottle* b =
bbb.find(
"origin:").asList();
348 if (
bbb.check(
"occupied_thresh:"))
349 {m_occupied_thresh =
bbb.find(
"occupied_thresh:").asFloat64();}
351 if (
bbb.check(
"free_thresh:"))
352 {m_free_thresh =
bbb.find(
"free_thresh:").asFloat64();}
396 for (
size_t y = 0; y <
m_height; y++)
398 for (
size_t x = 0; x <
m_width; x++)
405 for (
size_t y = 0; y <
m_height; y++)
407 for (
size_t x = 0; x <
m_width; x++)
409 m_map_occupancy.
safePixel(x, y) = PixelToCellOccupancyData(
ros_img.safePixel(x, y));
415 yError() <<
"MapGrid2D::loadFromFile() Size of YARP map and ROS do not match";
426 double resolution = 0;
452 for (
size_t y = 0; y <
m_height; y++)
454 for (
size_t x = 0; x <
m_width; x++)
456 m_map_occupancy.
safePixel(x, y) = PixelToCellOccupancyData(
ros_img.safePixel(x, y));
461 for (
size_t y = 0; y < (
size_t)(m_map_occupancy.
height()); y++)
463 for (
size_t x = 0; x < (
size_t)(m_map_occupancy.
width()); x++)
470 else {
yError() <<
"Unreachable";}
493 for (
size_t y = 0; y <
m_height; y++)
495 for (
size_t x = 0; x <
m_width; x++)
502 for (
size_t y = 0; y < (
size_t)(m_map_flags.
height()); y++)
504 for (
size_t x = 0; x < (
size_t)(m_map_flags.
width()); x++)
525 m_occupied_thresh = 0.80;
526 m_free_thresh = 0.20;
535 if (
mapfile.check(
"resolution"))
672 return (254 - (
cellin * 254 / 100));
692 else if (
pixin != (
unsigned char)(-1))
708 for (
size_t j=0;j<
height();j++){
709 for (
size_t i=0;i<
width();i++){
723 for (
int j=
height()-1; j>0; j--){
724 for (
int i=
width()-1; i>0 ;i--){
738 for (
size_t i=0;i<
width();i++){
739 for (
size_t j=0;j<
height();j++){
753 for (
size_t i=
width()-1;i>0;i--){
754 for (
size_t j=0;j<
height();j++){
766 if (left > (
int)this->
width()) {
769 if (right > (
int)this->
width()) {
772 if (top > (
int)this->
height()) {
775 if (bottom > (
int)this->
height()) {
790 for (
int j =
top, y = 0; j <
bottom; j++, y++) {
791 for (
int i=left, x=0; i<right; i++, x++)
799 this->m_width=m_map_occupancy.
width();
800 this->m_height=m_map_occupancy.
height();
837 yaml_file <<
"occupied_thresh: " << m_occupied_thresh <<
'\n';
838 yaml_file <<
"free_thresh: " << m_free_thresh <<
'\n';
847 for (
size_t y = 0; y <
m_height; y++)
849 for (
size_t x = 0; x <
m_width; x++)
889 char buff[255];
memset(buff, 0, 255);
893 m_compressed_data_over_network = connection.
expectInt8();
897 if (m_compressed_data_over_network)
899#if defined (YARP_HAS_ZLIB)
932 unsigned char* mem = m_map_occupancy.
getRawImage();
968 connection.
appendInt8(m_compressed_data_over_network);
970 if (m_compressed_data_over_network)
972#if defined (YARP_HAS_ZLIB)
1002 unsigned char* mem = m_map_occupancy.
getRawImage();
1027 yWarning() <<
"MapGrid2D::setOrigin() requested is not inside map!";
1042 yWarning() <<
"MapGrid2D::setOrigin() requested is not inside map!";
1057 if (resolution <= 0)
1059 yError() <<
"MapGrid2D::setResolution() invalid value:" << resolution;
1078 yError() <<
"MapGrid2D::setMapName() invalid map name";
1089 if (x <= 0 && y <= 0)
1091 yError() <<
"MapGrid2D::setSize() invalid size";
1096 yError() <<
"MapGrid2D::setSize() invalid map resolution.";
1107 if (x == 0 && y == 0)
1109 yError() <<
"MapGrid2D::setSize() invalid size";
1112 m_map_occupancy.
resize(x, y);
1113 m_map_flags.
resize(x, y);
1114 m_map_occupancy.
zero();
1137 yError() <<
"Invalid cell requested " <<
cell.x <<
" " <<
cell.y;
1148 yError() <<
"Invalid cell requested " <<
cell.x <<
" " <<
cell.y;
1159 yError() <<
"Invalid cell requested " <<
cell.x <<
" " <<
cell.y;
1177 yError() <<
"Invalid cell requested " <<
cell.x <<
" " <<
cell.y;
1196 yError() <<
"The size of given occupancy grid does not correspond to the current map. Use method setSize() first.";
1199 m_map_occupancy =
image;
1205 image = m_map_occupancy;
1211 for (
size_t y = 0; y <
m_height; y++)
1213 for (
size_t x = 0; x <
m_width; x++)
1228#if defined (YARP_HAS_ZLIB)
1229 m_compressed_data_over_network = val;
1232 yWarning() <<
"Zlib library not found, unable to set compression";
#define BOTTLE_TAG_FLOAT64
#define BOTTLE_TAG_STRING
static std::string extractExtensionFromFile(std::string full_filename)
static std::string extractPathFromFile(std::string full_filename)
contains the definition of a map type
bool isInsideMap(XYCell cell) const
Checks if a cell is inside the map.
MapGrid2DOrigin m_origin
pose of the map frame w.r.t. the bottom left corner of the map image
double m_resolution
meters/pixel
void setOrigin(double x_init, double y_init, double t_init)
void getSize_in_cells(size_t &x, size_t &y) const
Returns the size of the map in cells.
bool getMapFlag(XYCell cell, map_flags &flag) const
Get the flag of a specific cell of the map.
void clearMapTemporaryFlags()
Clear map temporary flags, such as: MAP_CELL_TEMPORARY_OBSTACLE, MAP_CELL_ENLARGED_OBSTACLE etc.
bool isIdenticalTo(const MapGrid2D &otherMap) const
Checks is two maps are identical.
bool getMapImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image) const
bool write(yarp::os::ConnectionWriter &connection) const override
Write vector to a connection.
bool read(yarp::os::ConnectionReader &connection) override
Read this object from a network connection.
size_t height() const
Retrieves the map height, expressed in cells.
bool setResolution(double resolution)
Sets the resolution of the map, i.e.
bool setOrigin(double x, double y, double theta)
Sets the origin of the map reference frame (according to ROS convention)
bool getOccupancyGrid(yarp::sig::ImageOf< yarp::sig::PixelMono > &image) const
std::string getMapName() const
Retrieves the map name.
bool setSize_in_cells(size_t x, size_t y)
Sets the size of the map in cells.
bool enable_map_compression_over_network(bool val)
void getOrigin(double &x, double &y, double &theta) const
Retrieves the origin of the map reference frame (according to ROS convention)
bool setMapImage(yarp::sig::ImageOf< yarp::sig::PixelRgb > &image)
bool setMapName(std::string map_name)
Sets the map name.
bool setMapFlag(XYCell cell, map_flags flag)
Set the flag of a specific cell of the map.
bool crop(int left, int top, int right, int bottom)
Modifies the map, cropping pixels at the boundaries.
bool isKeepOut(XYCell cell) const
Checks if a specific cell of the map is marked as keep-out.
bool isFree(XYCell cell) const
Checks if a specific cell of the map is free, i.e.
bool isNotFree(XYCell cell) const
Checks if a specific cell of the map contains is not free.
size_t width() const
Retrieves the map width, expressed in cells.
void getSize_in_meters(double &x, double &y) const
Returns the size of the map in meters, according to the current map resolution.
bool enlargeObstacles(double size)
Performs the obstacle enlargement operation.
bool setOccupancyData(XYCell cell, double occupancy)
Set the occupancy data of a specific cell of the map.
bool loadFromFile(std::string map_filename)
Loads a yarp map file from disk.
bool setSize_in_meters(double x, double y)
Sets the size of the map in meters, according to the current map resolution.
yarp::sig::PixelMono CellOccupancyData
bool setOccupancyGrid(yarp::sig::ImageOf< yarp::sig::PixelMono > &image)
bool saveToFile(std::string map_filename) const
Store a yarp map file to disk.
bool getOccupancyData(XYCell cell, double &occupancy) const
Retrieves the occupancy data of a specific cell of the map.
bool isWall(XYCell cell) const
Checks if a specific cell of the map contains a wall.
@ MAP_CELL_TEMPORARY_OBSTACLE
@ MAP_CELL_ENLARGED_OBSTACLE
void getResolution(double &resolution) const
Retrieves the resolution of the map, i.e.
A simple collection of objects that can be described and transmitted in a portable way.
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
A mini-server for performing network communication in the background.
void close() override
Stop port activity.
bool open(const std::string &name) override
Start port operation, with a specific name, with automatically-chosen network parameters.
An interface for reading from a network connection.
virtual bool expectBlock(char *data, size_t len)=0
Read a block of data from the network connection.
virtual std::int32_t expectInt32()=0
Read a 32-bit integer from the network connection.
virtual bool convertTextMode()=0
Reads in a standard description in text mode, and converts it to a standard description in binary.
virtual bool isError() const =0
virtual std::int8_t expectInt8()=0
Read a 8-bit integer from the network connection.
virtual yarp::conf::float64_t expectFloat64()=0
Read a 64-bit floating point number from the network connection.
An interface for writing to a network connection.
virtual bool isError() const =0
virtual void appendInt8(std::int8_t data)=0
Send a representation of a 8-bit integer to the network connection.
virtual void appendExternalBlock(const char *data, size_t len)=0
Send a block of data to the network connection, without making a copy.
virtual bool convertTextMode()=0
Converts a standard description in binary into a textual description, if the connection is in text-mo...
virtual void appendInt32(std::int32_t data)=0
Send a representation of a 32-bit integer to the network connection.
virtual void appendFloat64(yarp::conf::float64_t data)=0
Send a representation of a 64-bit floating point number to the network connection.
virtual void appendBlock(const char *data, size_t len)=0
Send a block of data to the network connection.
void appendString(const std::string &str)
Send a string to the network connection.
A class for storing options and configuration information.
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
virtual yarp::conf::float32_t asFloat32() const
Get 32-bit floating point value.
T & safePixel(size_t x, size_t y)
T & pixel(size_t x, size_t y)
void setQuantum(size_t imgQuantum)
size_t width() const
Gets width of image in pixels.
unsigned char * getRawImage() const
Access to the internal image buffer.
bool copy(const Image &alt)
Copy operator.
size_t getRawImageSize() const
Access to the internal buffer size information (this is how much memory has been allocated for the im...
void resize(size_t imgWidth, size_t imgHeight)
Reallocate an image to be of a desired size, throwing away its current contents.
void zero()
Set all pixels to 0.
size_t height() const
Gets height of image in pixels.
yarp::math::Vec2D< size_t > XYCell
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.
bool read(ImageOf< PixelRgb > &dest, const std::string &src, image_fileformat format=FORMAT_ANY)
bool write(const ImageOf< PixelRgb > &src, const std::string &dest, image_fileformat format=FORMAT_PPM)