YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
MapGrid2D.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-License-Identifier: BSD-3-Clause
4 */
5
6#define _USE_MATH_DEFINES
7
9
10#include <yarp/os/Bottle.h>
13#include <yarp/os/LogStream.h>
14#include <yarp/sig/ImageFile.h>
15#include <algorithm>
16#include <fstream>
17#include <filesystem>
18#include <cmath>
19
20#if defined (YARP_HAS_ZLIB)
21#include <zlib.h>
22#endif
23
24
25using namespace yarp::dev;
26using namespace yarp::dev::Nav2D;
27using namespace yarp::sig;
28using namespace yarp::os;
29using namespace yarp::math;
30
31#ifndef DEG2RAD
32#define DEG2RAD M_PI/180.0
33#endif
34
35#ifndef RAD2DEG
36#define RAD2DEG 180.0/M_PI
37#endif
38
39//helper functions
40static std::string extractPathFromFile(std::string full_filename)
41{
42 std::filesystem::path p(full_filename);
43 std::string path = p.parent_path().string();
44 if (path.empty()) return path;
45
46 path.push_back (std::filesystem::path::preferred_separator);
47 return path;
48}
49
50static std::string extractExtensionFromFile(std::string full_filename)
51{
52 int start = full_filename.length() - 3;
53 return full_filename.substr(start, 3);
54}
55
56
57bool MapGrid2D::isIdenticalTo(const MapGrid2D& other) const
58{
59 if (m_map_name != other.m_map_name) {
60 return false;
61 }
62 if (m_origin != other.m_origin) {
63 return false;
64 }
65 if (m_resolution != other.m_resolution) {
66 return false;
67 }
68 if (m_width != other.width()) {
69 return false;
70 }
71 if (m_height != other.height()) {
72 return false;
73 }
74 for (size_t y = 0; y < m_height; y++) {
75 for (size_t x = 0; x < m_width; x++) {
76 if (m_map_occupancy.safePixel(x, y) != other.m_map_occupancy.safePixel(x, y)) {
77 return false;
78 }
79 }
80 }
81 for (size_t y = 0; y < m_height; y++) {
82 for (size_t x = 0; x < m_width; x++) {
83 if (m_map_flags.safePixel(x, y) != other.m_map_flags.safePixel(x, y)) {
84 return false;
85 }
86 }
87 }
88 return true;
89}
90
92{
93 m_resolution = 1.0; //each pixel corresponds to 1 m
94 m_width = 2;
95 m_height = 2;
96 m_map_occupancy.setQuantum(1); //we do not want extra padding in map images
97 m_map_flags.setQuantum(1);
98 m_map_occupancy.resize(m_width, m_height);
99 m_map_flags.resize(m_width, m_height);
100 m_occupied_thresh = 0.80;
101 m_free_thresh = 0.20;
102 for (size_t y = 0; y < m_height; y++)
103 {
104 for (size_t x = 0; x < m_width; x++)
105 {
106 m_map_occupancy.safePixel(x, y) = 0;
108 }
109 }
110#if defined (YARP_HAS_ZLIB)
111 m_compressed_data_over_network = true;
112#else
113 m_compressed_data_over_network = false;
114#endif
115}
116
117MapGrid2D::~MapGrid2D() = default;
118
120{
121 if (isInsideMap(cell))
122 {
123 if (m_map_occupancy.safePixel(cell.x, cell.y) != 0) {
124 return true;
125 }
127 return true;
128 }
130 return true;
131 }
133 return true;
134 }
135 }
136 return false;
137}
138
140{
141 if (isInsideMap(cell))
142 {
143 if (m_map_occupancy.safePixel(cell.x, cell.y) == 0 && m_map_flags.safePixel(cell.x, cell.y) == MapGrid2D::map_flags::MAP_CELL_FREE) {
144 return true;
145 }
146 }
147 return false;
148}
149
151{
152 if (isInsideMap(cell))
153 {
155 return true;
156 }
157 }
158 return false;
159}
160
162{
163 if (isInsideMap(cell))
164 {
165 // if (m_map_occupancy.safePixel(cell.x, cell.y) == 0)
166 // return true;
167 if (m_map_flags.safePixel(cell.x, cell.y) == MapGrid2D::map_flags::MAP_CELL_WALL) {
168 return true;
169 }
170 }
171 return false;
172}
173
174size_t MapGrid2D::height() const
175{
176 return m_height;
177}
178
179size_t MapGrid2D::width() const
180{
181 return m_width;
182}
183
185{
186 image.setQuantum(1);
187 image.resize(m_width, m_height);
188 image.zero();
189 for (size_t y = 0; y < m_height; y++)
190 {
191 for (size_t x = 0; x < m_width; x++)
192 {
193 image.safePixel(x, y) = CellFlagDataToPixel(m_map_flags.safePixel(x, y));
194 }
195 }
196 return true;
197}
198
200{
201 if (image.width() != m_width ||
202 image.height() != m_height)
203 {
204 yError() << "The size of given image does not correspond to the current map. Use method setSize() first.";
205 return false;
206 }
207 for (size_t y = 0; y < m_height; y++)
208 {
209 for (size_t x = 0; x < m_width; x++)
210 {
211 m_map_flags.safePixel(x, y) = PixelToCellFlagData(image.safePixel(x, y));
212 }
213 }
214 return true;
215}
216
218{
219 if (size <= 0)
220 {
221 for (size_t y = 0; y < m_height; y++)
222 {
223 for (size_t x = 0; x < m_width; x++)
224 {
225 if (this->m_map_flags.safePixel(x, y) == MapGrid2D::map_flags::MAP_CELL_ENLARGED_OBSTACLE)
226 {
227 this->m_map_flags.safePixel(x, y) = MapGrid2D::map_flags::MAP_CELL_FREE;
228 }
229 }
230 }
231 return true;
232 }
233 auto repeat_num = (size_t)(std::ceil(size/ m_resolution));
234 for (size_t repeat = 0; repeat < repeat_num; repeat++)
235 {
236 //contains the cells to be enlarged;
237 std::vector<XYCell> list_of_cells;
238 for (size_t y = 0; y < m_height; y++)
239 {
240 for (size_t x = 0; x < m_width; x++)
241 {
242 //this check could be optimized...
243 if (this->m_map_flags.safePixel(x, y) == MAP_CELL_KEEP_OUT ||
244 this->m_map_flags.safePixel(x, y) == MAP_CELL_ENLARGED_OBSTACLE ||
245 this->m_map_flags.safePixel(x, y) == MAP_CELL_WALL ||
246 this->m_map_flags.safePixel(x, y) == MAP_CELL_UNKNOWN ||
247 this->m_map_flags.safePixel(x, y) == MAP_CELL_TEMPORARY_OBSTACLE)
248 {
249 list_of_cells.emplace_back(x, y);
250 }
251 }
252 }
253
254 //process each cell of the list and enlarges it
255 for (auto& list_of_cell : list_of_cells)
256 {
257 enlargeCell(list_of_cell);
258 }
259 }
260 return true;
261}
262
263void MapGrid2D::enlargeCell(XYCell cell)
264{
265 size_t i = cell.x;
266 size_t j = cell.y;
267 size_t il = cell.x > 1 ? cell.x - 1 : 0;
268 size_t ir = cell.x + 1 < (m_width)-1 ? cell.x + 1 : (m_width)-1;
269 size_t ju = cell.y > 1 ? cell.y - 1 : 0;
270 size_t jd = cell.y + 1 < (m_height)-1 ? cell.y + 1 : (m_height)-1;
271
272 if (m_map_flags.pixel(il, j) == MAP_CELL_FREE) {
273 m_map_flags.pixel(il, j) = MAP_CELL_ENLARGED_OBSTACLE;
274 }
275 if (m_map_flags.pixel(ir, j) == MAP_CELL_FREE) {
276 m_map_flags.pixel(ir, j) = MAP_CELL_ENLARGED_OBSTACLE;
277 }
278 if (m_map_flags.pixel(i, ju) == MAP_CELL_FREE) {
279 m_map_flags.pixel(i, ju) = MAP_CELL_ENLARGED_OBSTACLE;
280 }
281 if (m_map_flags.pixel(i, jd) == MAP_CELL_FREE) {
282 m_map_flags.pixel(i, jd) = MAP_CELL_ENLARGED_OBSTACLE;
283 }
284 if (m_map_flags.pixel(il, ju) == MAP_CELL_FREE) {
285 m_map_flags.pixel(il, ju) = MAP_CELL_ENLARGED_OBSTACLE;
286 }
287 if (m_map_flags.pixel(il, jd) == MAP_CELL_FREE) {
288 m_map_flags.pixel(il, jd) = MAP_CELL_ENLARGED_OBSTACLE;
289 }
290 if (m_map_flags.pixel(ir, ju) == MAP_CELL_FREE) {
291 m_map_flags.pixel(ir, ju) = MAP_CELL_ENLARGED_OBSTACLE;
292 }
293 if (m_map_flags.pixel(ir, jd) == MAP_CELL_FREE) {
294 m_map_flags.pixel(ir, jd) = MAP_CELL_ENLARGED_OBSTACLE;
295 }
296}
297
298bool MapGrid2D::loadROSParams(std::string ros_yaml_filename, std::string& pgm_occ_filename, double& resolution, double& orig_x, double& orig_y, double& orig_t )
299{
300 std::string file_string;
301 std::ifstream file;
302 file.open(ros_yaml_filename.c_str());
303 if (!file.is_open())
304 {
305 yError() << "failed to open file" << ros_yaml_filename;
306 return false;
307 }
308
309 std::string line;
310 while (getline(file, line))
311 {
312 if (line.find("origin") != std::string::npos)
313 {
314 std::replace(line.begin(), line.end(), ',', ' ');
315 std::replace(line.begin(), line.end(), '[', '(');
316 std::replace(line.begin(), line.end(), ']', ')');
317 /*
318 auto it = line.find('[');
319 if (it != std::string::npos) line.replace(it, 1, "(");
320 it = line.find(']');
321 if(it != std::string::npos) line.replace(it, 1, ")");*/
322 }
323 file_string += (line + '\n');
324 }
325 file.close();
326
327 bool ret = true;
328 Bottle bbb;
329 bbb.fromString(file_string);
330 std::string debug_s = bbb.toString();
331
332 if (bbb.check("image:") == false) { yError() << "missing image"; ret = false; }
333 pgm_occ_filename = bbb.find("image:").asString();
334 //ppm_flg_filename = (pgm_occ_filename.substr(0, pgm_occ_filename.size()-4))+"_yarpflags"+".ppm";
335
336 if (bbb.check("resolution:") == false) { yError() << "missing resolution"; ret = false; }
337 resolution = bbb.find("resolution:").asFloat64();
338
339 if (bbb.check("origin:") == false) { yError() << "missing origin"; ret = false; }
340 Bottle* b = bbb.find("origin:").asList();
341 if (b)
342 {
343 orig_x = b->get(0).asFloat64();
344 orig_y = b->get(1).asFloat64();
345 orig_t = b->get(2).asFloat64() * RAD2DEG;
346 }
347
348 if (bbb.check("occupied_thresh:"))
349 {m_occupied_thresh = bbb.find("occupied_thresh:").asFloat64();}
350
351 if (bbb.check("free_thresh:"))
352 {m_free_thresh = bbb.find("free_thresh:").asFloat64();}
353
354 return ret;
355}
356
357bool MapGrid2D::loadMapYarpAndRos(std::string yarp_filename, std::string ros_yaml_filename)
358{
362 if (b1 == false)
363 {
364 yError() << "Unable to load map data" << yarp_filename;
365 return false;
366 }
367 std::string pgm_occ_filename;
368 double resolution=0;
369 double orig_x = 0;
370 double orig_y = 0;
371 double orig_t = 0;
372 bool b2 = loadROSParams(ros_yaml_filename, pgm_occ_filename, resolution, orig_x, orig_y, orig_t);
373 if (b2 == false)
374 {
375 yError() << "Unable to ros params from" << ros_yaml_filename;
376 return false;
377 }
378 std::string path = extractPathFromFile(ros_yaml_filename);
380 std::string pgm_occ_filename_with_path = path + pgm_occ_filename;
382 if (b3 == false)
383 {
384 yError() << "Unable to load occupancy grid file:" << pgm_occ_filename_with_path;
385 return false;
386 }
387
388 if (yarp_img.width() == ros_img.width() && yarp_img.height() == ros_img.height())
389 {
390 //Everything ok, proceed to internal assignments
391 setSize_in_cells(yarp_img.width(), yarp_img.height());
392 m_resolution = resolution;
394
395 //set YARPS stuff
396 for (size_t y = 0; y < m_height; y++)
397 {
398 for (size_t x = 0; x < m_width; x++)
399 {
400 m_map_flags.safePixel(x, y) = PixelToCellFlagData(yarp_img.safePixel(x, y));
401 }
402 }
403
404 //set ROS Stuff
405 for (size_t y = 0; y < m_height; y++)
406 {
407 for (size_t x = 0; x < m_width; x++)
408 {
409 m_map_occupancy.safePixel(x, y) = PixelToCellOccupancyData(ros_img.safePixel(x, y));
410 }
411 }
412 }
413 else
414 {
415 yError() << "MapGrid2D::loadFromFile() Size of YARP map and ROS do not match";
416 return false;
417 }
418
419 return true;
420}
421
422bool MapGrid2D::loadMapROSOnly(std::string ros_yaml_filename)
423{
425 std::string pgm_occ_filename;
426 double resolution = 0;
427 double orig_x = 0;
428 double orig_y = 0;
429 double orig_t = 0;
430 bool b2 = loadROSParams(ros_yaml_filename, pgm_occ_filename, resolution, orig_x, orig_y, orig_t);
431 if (b2 == false)
432 {
433 yError() << "Unable to ros params from" << ros_yaml_filename;
434 return false;
435 }
436 std::string path = extractPathFromFile(ros_yaml_filename);
438 std::string pgm_occ_filename_with_path = path + pgm_occ_filename;
440 if (b3 == false)
441 {
442 yError() << "Unable to load occupancy grid file:" << pgm_occ_filename;
443 return false;
444 }
445
446 //Everything ok, proceed to internal assignments
447 setSize_in_cells(ros_img.width(), ros_img.height());
448 m_resolution = resolution;
450
451 //set ROS Stuff
452 for (size_t y = 0; y < m_height; y++)
453 {
454 for (size_t x = 0; x < m_width; x++)
455 {
456 m_map_occupancy.safePixel(x, y) = PixelToCellOccupancyData(ros_img.safePixel(x, y));
457 }
458 }
459
460 //generate YARP stuff from ROS Stuff
461 for (size_t y = 0; y < (size_t)(m_map_occupancy.height()); y++)
462 {
463 for (size_t x = 0; x < (size_t)(m_map_occupancy.width()); x++)
464 {
465 //occup_prob is a probability, in the range 0-100 (-1 = 255 = unknown)
466 CellOccupancyData occup_prob = m_map_occupancy.safePixel(x, y);
467 if (occup_prob != (unsigned char)(-1) && occup_prob<50 ) {m_map_flags.safePixel(x, y) = MAP_CELL_FREE;}
468 else if (occup_prob >= 50 && occup_prob<=100) {m_map_flags.safePixel(x, y) = MAP_CELL_WALL;}
469 else if (occup_prob > 100) {m_map_flags.safePixel(x, y) = MAP_CELL_UNKNOWN;}
470 else { yError() << "Unreachable";}
471 }
472 }
473 return true;
474}
475
476bool MapGrid2D::loadMapYarpOnly(std::string yarp_filename)
477{
480 if (b1 == false)
481 {
482 yError() << "Unable to load map" << yarp_filename;
483 return false;
484 }
485 //Everything ok, proceed to internal assignments
486 setSize_in_cells(yarp_img.width(), yarp_img.height());
487 //m_resolution = resolution; //????
488 //m_origin.x = orig_x; //????
489 //m_origin.y = orig_y; //????
490 //m_origin.theta = orig_t; //????
491
492 //set YARPS stuff
493 for (size_t y = 0; y < m_height; y++)
494 {
495 for (size_t x = 0; x < m_width; x++)
496 {
497 m_map_flags.safePixel(x, y) = PixelToCellFlagData(yarp_img.safePixel(x, y));
498 }
499 }
500
501 //generate ROS stuff from YARP Stuff
502 for (size_t y = 0; y < (size_t)(m_map_flags.height()); y++)
503 {
504 for (size_t x = 0; x < (size_t)(m_map_flags.width()); x++)
505 {
506 yarp::sig::PixelMono pix_flg = m_map_flags.safePixel(x, y);
507
508 if (pix_flg == MAP_CELL_FREE) {
509 m_map_occupancy.safePixel(x, y) = 0; //@@@SET HERE
510 } else if (pix_flg == MAP_CELL_KEEP_OUT) {
511 m_map_occupancy.safePixel(x, y) = 0; //@@@SET HERE
512 } else if (pix_flg == MAP_CELL_TEMPORARY_OBSTACLE) {
513 m_map_occupancy.safePixel(x, y) = 0; //@@@SET HERE
514 } else if (pix_flg == MAP_CELL_ENLARGED_OBSTACLE) {
515 m_map_occupancy.safePixel(x, y) = 0; //@@@SET HERE
516 } else if (pix_flg == MAP_CELL_WALL) {
517 m_map_occupancy.safePixel(x, y) = 100; //@@@SET HERE
518 } else if (pix_flg == MAP_CELL_UNKNOWN) {
519 m_map_occupancy.safePixel(x, y) = 255; //@@@SET HERE
520 } else {
521 m_map_occupancy.safePixel(x, y) = 255; //@@@SET HERE
522 }
523 }
524 }
525 m_occupied_thresh = 0.80; //@@@SET HERE
526 m_free_thresh = 0.20;//@@@SET HERE
527 return true;
528}
529
530bool MapGrid2D::parseMapParameters(const Property& mapfile)
531{
532 //Map parameters.
533 //these values can eventually override values previously assigned
534 //(e.g. ROS values found in yaml data)
535 if (mapfile.check("resolution"))
536 {
537 m_resolution = mapfile.find("resolution").asFloat32();
538 }
539 if (mapfile.check("origin"))
540 {
541 Bottle* b = mapfile.find("origin").asList();
542 if (b)
543 {
545 b->get(1).asFloat32(),
546 b->get(2).asFloat32());
547 }
548 }
549
550 return true;
551}
552
554{
557 if (mapfile_prop.fromConfigFile(map_file_with_path) == false)
558 {
559 yError() << "Unable to open .map description file:" << map_file_with_path;
560 return false;
561 }
562
563 if (mapfile_prop.check("MapName") ==false)
564 {
565 yError() << "Unable to find 'MapName' parameter inside:" << map_file_with_path;
566 return false;
567 }
568 m_map_name = mapfile_prop.find("MapName").asString();
569
570 bool YarpMapDataFound = false;
571 std::string ppm_flg_filename;
572 if (mapfile_prop.check("YarpMapData") == false)
573 {
574 yWarning() << "Unable to find 'YarpMapData' parameter inside:" << map_file_with_path;
575 YarpMapDataFound = false;
576 }
577 else
578 {
579 ppm_flg_filename = mapfile_prop.find("YarpMapData").asString();
580 YarpMapDataFound = true;
581 }
582
583 bool RosMapDataFound = false;
584 std::string yaml_filename;
585 if (mapfile_prop.check("RosMapData") == false)
586 {
587 yWarning() << "Unable to find 'RosMapData' parameter inside:" << map_file_with_path;
588 RosMapDataFound = false;
589 }
590 else
591 {
592 yaml_filename = mapfile_prop.find("RosMapData").asString();
593 RosMapDataFound = true;
594 }
595
596 m_width = -1;
597 m_height = -1;
601 {
602 //yarp and ros
603 yDebug() << "Opening files: "<< yarp_flg_filename_with_path << " and " << ros_yaml_filename_with_path;
604 return this->loadMapYarpAndRos(yarp_flg_filename_with_path, ros_yaml_filename_with_path) &&
605 this->parseMapParameters(mapfile_prop);
606 }
608 {
609 //only ros
610 yDebug() << "Opening file:" << ros_yaml_filename_with_path;
611 return this->loadMapROSOnly(ros_yaml_filename_with_path) &&
612 this->parseMapParameters(mapfile_prop);
613 }
615 {
616 //only yarp
617 yDebug() << "Opening file:" << yarp_flg_filename_with_path;
618 return this->loadMapYarpOnly(yarp_flg_filename_with_path) &&
619 this->parseMapParameters(mapfile_prop);
620 }
621 else
622 {
623 yError() << "Critical error: unable to find neither 'RosMapData' nor 'YarpMapData' inside:" << map_file_with_path;
624 return false;
625 }
626 return true;
627}
628
629MapGrid2D::CellFlagData MapGrid2D::PixelToCellFlagData(const yarp::sig::PixelRgb& pixin) const
630{
631 if (pixin.r == 0 && pixin.g == 0 && pixin.b == 0) {
632 return MAP_CELL_WALL;
633 } else if (pixin.r == 205 && pixin.g == 205 && pixin.b == 205) {
634 return MAP_CELL_UNKNOWN;
635 } else if (pixin.r == 254 && pixin.g == 254 && pixin.b == 254) {
636 return MAP_CELL_FREE;
637 } else if (pixin.r == 255 && pixin.g == 0 && pixin.b == 0) {
638 return MAP_CELL_KEEP_OUT;
639 }
640 return MAP_CELL_UNKNOWN;
641}
642
643yarp::sig::PixelRgb MapGrid2D::CellFlagDataToPixel(const MapGrid2D::CellFlagData& cellin) const
644{
645 yarp::sig::PixelRgb pixout_flg;
646 if (cellin == MAP_CELL_WALL) { pixout_flg.r = 0; pixout_flg.g = 0; pixout_flg.b = 0;}
647 else if (cellin == MAP_CELL_UNKNOWN) { pixout_flg.r = 205; pixout_flg.g = 205; pixout_flg.b = 205; }
648 else if (cellin == MAP_CELL_FREE) { pixout_flg.r = 254; pixout_flg.g = 254; pixout_flg.b = 254; }
649 else if (cellin == MAP_CELL_KEEP_OUT) { pixout_flg.r = 255; pixout_flg.g = 0; pixout_flg.b = 0; }
650 else if (cellin == MAP_CELL_ENLARGED_OBSTACLE) { pixout_flg.r = 255; pixout_flg.g = 200; pixout_flg.b = 0; }
651 else if (cellin == MAP_CELL_TEMPORARY_OBSTACLE) { pixout_flg.r = 100; pixout_flg.g = 100; pixout_flg.b = 200; }
652 else
653 {
654 //invalid
655 pixout_flg.r = 200; pixout_flg.g = 0; pixout_flg.b = 200;
656 }
657 return pixout_flg;
658}
659
660yarp::sig::PixelMono MapGrid2D::CellOccupancyDataToPixel(const MapGrid2D::CellOccupancyData& cellin) const
661{
662 //convert from value to image
663
664 //255 (-1) stands for unknown
665 if (cellin == 255)
666 {
667 return 205;
668 }
669 //values in the range 0-100 are converted in the range 0-254
670 else if (cellin != (unsigned char)(-1) && cellin <=100)
671 {
672 return (254 - (cellin * 254 / 100));
673 }
674 else
675 {
676 //invalid values are in the range 100-255.
677 //return invalid value 205
678 return 205;
679 }
680}
681
682MapGrid2D::CellOccupancyData MapGrid2D::PixelToCellOccupancyData(const yarp::sig::PixelMono& pixin) const
683{
684 //convert from image to value
685
686 //205 is a special code, used for unknown
687 if (pixin == 205)
688 {
689 return 255;
690 }
691 //values in the range 0-254 are converted in the range 0-100
692 else if (pixin != (unsigned char)(-1))
693 {
694 auto occ = (unsigned char)((254 - pixin) / 254.0);
695 return occ * 100;
696 }
697 else
698 {
699 //255 is an invalid value
700 return 255;
701 }
702}
703
704bool MapGrid2D::crop (int left, int top, int right, int bottom)
705{
706 if (top < 0)
707 {
708 for (size_t j=0;j<height();j++){
709 for (size_t i=0;i<width();i++){
710 yarp::sig::PixelMono pix = m_map_occupancy.safePixel(i,j);
711 if ( pix != 255)
712 {
713 top = j;
714 goto topFound;
715 }
716 }
717 }
718 }
719 topFound:
720
721 if (bottom < 0)
722 {
723 for (int j=height()-1; j>0; j--){
724 for (int i=width()-1; i>0 ;i--){
725 yarp::sig::PixelMono pix = m_map_occupancy.safePixel(i,j);
726 if ( pix != 255)
727 {
728 bottom = j+1;
729 goto bottomFound;
730 }
731 }
732 }
733 }
735
736 if (left<0)
737 {
738 for (size_t i=0;i<width();i++){
739 for (size_t j=0;j<height();j++){
740 yarp::sig::PixelMono pix = m_map_occupancy.safePixel(i,j);
741 if ( pix != 255)
742 {
743 left = i;
744 goto leftFound;
745 }
746 }
747 }
748 }
749 leftFound:
750
751 if (right<0)
752 {
753 for (size_t i=width()-1;i>0;i--){
754 for (size_t j=0;j<height();j++){
755 yarp::sig::PixelMono pix = m_map_occupancy.safePixel(i,j);
756 if ( pix != 255)
757 {
758 right = i;
759 goto rightFound;
760 }
761 }
762 }
763 }
765
766 if (left > (int)this->width()) {
767 return false;
768 }
769 if (right > (int)this->width()) {
770 return false;
771 }
772 if (top > (int)this->height()) {
773 return false;
774 }
775 if (bottom > (int)this->height()) {
776 return false;
777 }
778
781
782 new_map_occupancy.setQuantum(1);
783 new_map_flags.setQuantum(1);
784 new_map_occupancy.resize(right-left,bottom-top);
785 new_map_flags.resize(right-left,bottom-top);
786
787// size_t original_width = m_map_occupancy.width();
788 size_t original_height = m_map_occupancy.height();
789
790 for (int j = top, y = 0; j < bottom; j++, y++) {
791 for (int i=left, x=0; i<right; i++, x++)
792 {
793 new_map_occupancy.safePixel(x,y) = m_map_occupancy.safePixel(i,j);
794 new_map_flags.safePixel(x,y) = m_map_flags.safePixel(i,j);
795 }
796 }
797 m_map_occupancy.copy(new_map_occupancy);
798 m_map_flags.copy(new_map_flags);
799 this->m_width=m_map_occupancy.width();
800 this->m_height=m_map_occupancy.height();
801 yDebug() << m_origin.get_x() << m_origin.get_y();
802 double new_x0 = m_origin.get_x() +(left*m_resolution);
805 return true;
806}
807
809{
811
812 std::string yarp_filename = this->getMapName() + "_yarpflags.ppm";
813 std::string yaml_filename = this->getMapName() + "_grid.yaml";
814 std::string pgm_occ_filename = this->getMapName() + "_grid.pgm";
815
816 std::ofstream map_file;
818 if (!map_file.is_open())
819 {
820 return false;
821 }
822 map_file << "MapName "<< this->getMapName() << '\n';
823 map_file << "YarpMapData "<< yarp_filename << '\n';
824 map_file << "RosMapData "<< yaml_filename << '\n';
825 map_file.close();
826
827 std::ofstream yaml_file;
829 if (!yaml_file.is_open())
830 {
831 return false;
832 }
833 yaml_file << "image: " << pgm_occ_filename << '\n';
834 yaml_file << "resolution: " << m_resolution << '\n';
835 yaml_file << "origin: [ " << m_origin.get_x() << " " << m_origin.get_y() << " " << m_origin.get_theta() << " ]"<< '\n';
836 yaml_file << "negate: 0" << '\n';
837 yaml_file << "occupied_thresh: " << m_occupied_thresh << '\n';
838 yaml_file << "free_thresh: " << m_free_thresh << '\n';
839
841
844
845 img_flg.resize(m_width, m_height);
846 img_occ.resize(m_width, m_height);
847 for (size_t y = 0; y < m_height; y++)
848 {
849 for (size_t x = 0; x < m_width; x++)
850 {
851 yarp::sig::PixelMono pix_flg = m_map_flags.safePixel(x, y);
852 yarp::sig::PixelMono pix_occ = m_map_occupancy.safePixel(x,y);
853 img_flg.safePixel(x, y) = CellFlagDataToPixel(pix_flg);
854 img_occ.safePixel(x, y) = CellOccupancyDataToPixel(pix_occ);
855 }
856 }
857
858 //std::string ppm_flg_filename = (pgm_occ_filename.substr(0, pgm_occ_filename.size() - 4)) + "_yarpflags" + ".ppm";
859 std::string ppm_flg_filename = yarp_filename;
860 bool ret = true;
863 return ret;
864}
865
867{
868 // auto-convert text mode interaction
869 connection.convertTextMode();
870
871 connection.expectInt32();
872 connection.expectInt32();
873
874 connection.expectInt32();
875 m_width = connection.expectInt32();
876 connection.expectInt32();
877 m_height = connection.expectInt32();
878 connection.expectInt32();
879 double x0 = connection.expectFloat64();
880 connection.expectInt32();
881 double y0 = connection.expectFloat64();
882 connection.expectInt32();
883 double theta0 = connection.expectFloat64();
885 connection.expectInt32();
886 m_resolution = connection.expectFloat64();
887 connection.expectInt32();
888 int siz = connection.expectInt32();
889 char buff[255]; memset(buff, 0, 255);
890 connection.expectBlock((char*)buff, siz);
891 m_map_name = buff;
892 connection.expectInt32();
893 m_compressed_data_over_network = connection.expectInt8();
894 m_map_occupancy.resize(m_width, m_height);
895 m_map_flags.resize(m_width, m_height);
896
897 if (m_compressed_data_over_network)
898 {
899#if defined (YARP_HAS_ZLIB)
900 bool ok = true;
901 {
902 connection.expectInt32();
903 size_t sizeCompressed = connection.expectInt32();
904 unsigned char* dataCompressed = new unsigned char[sizeCompressed];
905 ok &= connection.expectBlock((char*)dataCompressed, sizeCompressed);
906 size_t sizeUncompressed = m_map_occupancy.getRawImageSize();
907 unsigned char* dataUncompressed = m_map_occupancy.getRawImage();
910 delete[] dataCompressed;
911 }
912 {
913 connection.expectInt32();
914 size_t sizeCompressed = connection.expectInt32();
915 unsigned char* dataCompressed = new unsigned char[sizeCompressed];
916 ok &= connection.expectBlock((char*)dataCompressed, sizeCompressed);
917 size_t sizeUncompressed = m_map_flags.getRawImageSize();
918 unsigned char* dataUncompressed = m_map_flags.getRawImage();
921 delete[] dataCompressed;
922 }
923#endif
924 }
925 else
926 {
927 bool ok = true;
928 {
929 connection.expectInt32();
930 size_t memsize = connection.expectInt32();
931 if (memsize != m_map_occupancy.getRawImageSize()) { return false; }
932 unsigned char* mem = m_map_occupancy.getRawImage();
933 ok &= connection.expectBlock((char*)mem, memsize);
934 }
935 {
936 connection.expectInt32();
937 size_t memsize = connection.expectInt32();
938 if (memsize != m_map_flags.getRawImageSize()) { return false; }
939 unsigned char* mem = m_map_flags.getRawImage();
940 ok &= connection.expectBlock((char*)mem, memsize);
941 }
942 if (!ok) {
943 return false;
944 }
945 }
946 return !connection.isError();
947}
948
950{
951 connection.appendInt32(BOTTLE_TAG_LIST);
952 connection.appendInt32(10);
953 connection.appendInt32(BOTTLE_TAG_INT32); //1
954 connection.appendInt32(m_width);
955 connection.appendInt32(BOTTLE_TAG_INT32); //2
956 connection.appendInt32(m_height);
957 connection.appendInt32(BOTTLE_TAG_FLOAT64); // 3
958 connection.appendFloat64(m_origin.get_x());
959 connection.appendInt32(BOTTLE_TAG_FLOAT64); // 4
960 connection.appendFloat64(m_origin.get_y());
961 connection.appendInt32(BOTTLE_TAG_FLOAT64); // 5
962 connection.appendFloat64(m_origin.get_theta());
963 connection.appendInt32(BOTTLE_TAG_FLOAT64); // 6
964 connection.appendFloat64(m_resolution);
965 connection.appendInt32(BOTTLE_TAG_STRING); // 7
966 connection.appendString(m_map_name);
967 connection.appendInt32(BOTTLE_TAG_INT8); // 8
968 connection.appendInt8(m_compressed_data_over_network);
969
970 if (m_compressed_data_over_network)
971 {
972#if defined (YARP_HAS_ZLIB)
973 {
974 size_t sizeUncompressed = m_map_occupancy.getRawImageSize();
975 unsigned char* dataUncompressed = m_map_occupancy.getRawImage();
977 unsigned char* dataCompressed = new unsigned char[sizeCompressed];
980 connection.appendInt32(BOTTLE_TAG_BLOB); // 9
981 connection.appendInt32(sizeCompressed);
982 connection.appendBlock((char*)dataCompressed, sizeCompressed);
983 delete [] dataCompressed;
984 }
985 {
986 size_t sizeUncompressed = m_map_flags.getRawImageSize();
987 unsigned char* dataUncompressed = m_map_flags.getRawImage();
989 unsigned char* dataCompressed = new unsigned char[sizeCompressed];
992 connection.appendInt32(BOTTLE_TAG_BLOB); // 10
993 connection.appendInt32(sizeCompressed);
994 connection.appendBlock((char*)dataCompressed, sizeCompressed);
995 delete[] dataCompressed;
996 }
997#endif
998 }
999 else
1000 {
1001 {
1002 unsigned char* mem = m_map_occupancy.getRawImage();
1003 int memsize = m_map_occupancy.getRawImageSize();
1004 connection.appendInt32(BOTTLE_TAG_BLOB);
1005 connection.appendInt32(memsize);
1006 connection.appendExternalBlock((char*)mem, memsize);
1007 }
1008 {
1009 unsigned char* mem = m_map_flags.getRawImage();
1010 int memsize = m_map_flags.getRawImageSize();
1011 connection.appendInt32(BOTTLE_TAG_BLOB);
1012 connection.appendInt32(memsize);
1013 connection.appendExternalBlock((char*)mem, memsize);
1014 }
1015 }
1016
1017 connection.convertTextMode();
1018 return !connection.isError();
1019}
1020
1021bool MapGrid2D::setOrigin(double x, double y, double theta)
1022{
1023 //the given x and y are referred to the bottom left corner, pointing outwards.
1024 //To check if it is inside the map, I have to convert it to a cell with x and y referred to the upper left corner, pointing inwards
1025 if (m_resolution<=0)
1026 {
1027 yWarning() << "MapGrid2D::setOrigin() requested is not inside map!";
1028 return false;
1029 }
1030
1031 int xc = (int)(x/ m_resolution);
1032 int yc = (int)(y / m_resolution);
1033
1034 XYCell orig(-xc, (m_height-1) + yc);
1035 if (isInsideMap(orig))
1036 {
1037 m_origin.setOrigin(x,y, theta);
1038 return true;
1039 }
1040 else
1041 {
1042 yWarning() << "MapGrid2D::setOrigin() requested is not inside map!";
1043 m_origin.setOrigin(x, y, theta);
1044 return true;
1045 }
1046}
1047
1048void MapGrid2D::getOrigin(double& x, double& y, double& theta) const
1049{
1050 x = m_origin.get_x();
1051 y = m_origin.get_y();
1052 theta = m_origin.get_theta();
1053}
1054
1055bool MapGrid2D::setResolution(double resolution)
1056{
1057 if (resolution <= 0)
1058 {
1059 yError() << "MapGrid2D::setResolution() invalid value:" << resolution;
1060 return false;
1061 }
1062 m_resolution = resolution;
1063 return true;
1064}
1065
1066void MapGrid2D::getResolution(double& resolution) const
1067{
1068 resolution = m_resolution;
1069}
1070
1071bool MapGrid2D::setMapName(std::string map_name)
1072{
1073 if (map_name != "")
1074 {
1075 m_map_name = map_name;
1076 return true;
1077 }
1078 yError() << "MapGrid2D::setMapName() invalid map name";
1079 return false;
1080}
1081
1082std::string MapGrid2D::getMapName() const
1083{
1084 return m_map_name;
1085}
1086
1087bool MapGrid2D::setSize_in_meters(double x, double y)
1088{
1089 if (x <= 0 && y <= 0)
1090 {
1091 yError() << "MapGrid2D::setSize() invalid size";
1092 return false;
1093 }
1094 if (m_resolution <= 0)
1095 {
1096 yError() << "MapGrid2D::setSize() invalid map resolution.";
1097 return false;
1098 }
1099 auto w = (size_t)(x/m_resolution);
1100 auto h = (size_t)(y/m_resolution);
1101 setSize_in_cells(w,h);
1102 return true;
1103}
1104
1105bool MapGrid2D::setSize_in_cells(size_t x, size_t y)
1106{
1107 if (x == 0 && y == 0)
1108 {
1109 yError() << "MapGrid2D::setSize() invalid size";
1110 return false;
1111 }
1112 m_map_occupancy.resize(x, y);
1113 m_map_flags.resize(x, y);
1114 m_map_occupancy.zero();
1115 m_map_flags.zero();
1116 m_width = x;
1117 m_height = y;
1118 return true;
1119}
1120
1121void MapGrid2D::getSize_in_meters(double& x, double& y) const
1122{
1123 x = m_width* m_resolution;
1125}
1126
1127void MapGrid2D::getSize_in_cells(size_t&x, size_t& y) const
1128{
1129 x = m_width;
1130 y = m_height;
1131}
1132
1134{
1135 if (isInsideMap(cell) == false)
1136 {
1137 yError() << "Invalid cell requested " << cell.x << " " << cell.y;
1138 return false;
1139 }
1140 m_map_flags.safePixel(cell.x, cell.y) = flag;
1141 return true;
1142}
1143
1145{
1146 if (isInsideMap(cell) == false)
1147 {
1148 yError() << "Invalid cell requested " << cell.x << " " << cell.y;
1149 return false;
1150 }
1151 flag = (MapGrid2D::map_flags) m_map_flags.safePixel(cell.x, cell.y);
1152 return true;
1153}
1154
1156{
1157 if (isInsideMap(cell) == false)
1158 {
1159 yError() << "Invalid cell requested " << cell.x << " " << cell.y;
1160 return false;
1161 }
1162 if (occupancy <0)
1163 {
1164 m_map_occupancy.safePixel(cell.x, cell.y) = 255;
1165 }
1166 else
1167 {
1168 m_map_occupancy.safePixel(cell.x, cell.y) = (yarp::sig::PixelMono)(occupancy);
1169 }
1170 return true;
1171}
1172
1174{
1175 if (isInsideMap(cell) == false)
1176 {
1177 yError() << "Invalid cell requested " << cell.x << " " << cell.y;
1178 return false;
1179 }
1180 if (m_map_occupancy.safePixel(cell.x, cell.y)==255)
1181 {
1182 occupancy =-1;
1183 }
1184 else
1185 {
1186 occupancy = m_map_occupancy.safePixel(cell.x, cell.y);
1187 }
1188 return true;
1189}
1190
1192{
1193 if ((size_t) image.width() != m_width ||
1194 (size_t) image.height() != m_height)
1195 {
1196 yError() << "The size of given occupancy grid does not correspond to the current map. Use method setSize() first.";
1197 return false;
1198 }
1199 m_map_occupancy = image;
1200 return true;
1201}
1202
1204{
1205 image = m_map_occupancy;
1206 return true;
1207}
1208
1210{
1211 for (size_t y = 0; y < m_height; y++)
1212 {
1213 for (size_t x = 0; x < m_width; x++)
1214 {
1215 switch (m_map_flags.safePixel(x, y))
1216 {
1219 m_map_flags.safePixel(x, y) = MAP_CELL_FREE;
1220 break;
1221 }
1222 }
1223 }
1224}
1225
1227{
1228#if defined (YARP_HAS_ZLIB)
1229 m_compressed_data_over_network = val;
1230 return true;
1231#else
1232 yWarning() << "Zlib library not found, unable to set compression";
1233 return false;
1234#endif
1235}
#define BOTTLE_TAG_INT8
Definition Bottle.h:19
#define BOTTLE_TAG_FLOAT64
Definition Bottle.h:26
#define BOTTLE_TAG_INT32
Definition Bottle.h:21
#define BOTTLE_TAG_STRING
Definition Bottle.h:27
#define BOTTLE_TAG_BLOB
Definition Bottle.h:28
#define BOTTLE_TAG_LIST
Definition Bottle.h:29
#define yError(...)
Definition Log.h:361
#define yDebug(...)
Definition Log.h:275
#define yWarning(...)
Definition Log.h:340
static std::string extractExtensionFromFile(std::string full_filename)
Definition MapGrid2D.cpp:50
static std::string extractPathFromFile(std::string full_filename)
Definition MapGrid2D.cpp:40
#define RAD2DEG
Definition MapGrid2D.cpp:36
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.
Definition MapGrid2D.cpp:57
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
Definition MapGrid2D.h:30
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.
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.
Definition Bottle.h:64
Value & get(size_type index) const
Reads a Value v from a certain part of the list.
Definition Bottle.cpp:247
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.
Definition Property.h:32
virtual yarp::conf::float64_t asFloat64() const
Get 64-bit floating point value.
Definition Value.cpp:222
virtual yarp::conf::float32_t asFloat32() const
Get 32-bit floating point value.
Definition Value.cpp:216
Typed image class.
Definition Image.h:603
T & safePixel(size_t x, size_t y)
Definition Image.h:635
T & pixel(size_t x, size_t y)
Definition Image.h:619
void setQuantum(size_t imgQuantum)
Definition Image.cpp:435
size_t width() const
Gets width of image in pixels.
Definition Image.h:171
unsigned char * getRawImage() const
Access to the internal image buffer.
Definition Image.cpp:462
bool copy(const Image &alt)
Copy operator.
Definition Image.cpp:611
size_t getRawImageSize() const
Access to the internal buffer size information (this is how much memory has been allocated for the im...
Definition Image.cpp:471
void resize(size_t imgWidth, size_t imgHeight)
Reallocate an image to be of a desired size, throwing away its current contents.
Definition Image.cpp:387
void zero()
Set all pixels to 0.
Definition Image.cpp:380
size_t height() const
Gets height of image in pixels.
Definition Image.h:177
yarp::math::Vec2D< size_t > XYCell
Definition NavTypes.h:16
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)
#define YARP_UNUSED(var)
Definition api.h:162