Skip to content

Commit 7fdd8a5

Browse files
doisygGuillaume Doisy
authored and
SteveMacenski
committed
[map_io] Replace std logs by rclcpp logs (#4720)
* replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy <guillaume@dexory.com> * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy <guillaume@dexory.com> --------- Signed-off-by: Guillaume Doisy <guillaume@dexory.com> Co-authored-by: Guillaume Doisy <guillaume@dexory.com>
1 parent 231c88a commit 7fdd8a5

File tree

1 file changed

+87
-63
lines changed

1 file changed

+87
-63
lines changed

‎nav2_map_server/src/map_io.cpp‎

Lines changed: 87 additions & 63 deletions
Original file line numberDiff line numberDiff line change
@@ -132,8 +132,10 @@ std::string expand_user_home_dir_if_needed(
132132
return yaml_filename;
133133
}
134134
if (home_variable_value.empty()) {
135-
std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n"
136-
<< "[INFO] [map_io] User home dir will be not expanded \n";
135+
RCLCPP_INFO_STREAM(
136+
rclcpp::get_logger(
137+
"map_io"), "Map yaml file name starts with '~/' but no HOME variable set. \n"
138+
<< "[INFO] [map_io] User home dir will be not expanded \n");
137139
return yaml_filename;
138140
}
139141
const std::string prefix{home_variable_value};
@@ -181,15 +183,18 @@ LoadParameters loadMapYaml(const std::string & yaml_filename)
181183
load_parameters.negate = yaml_get_value<bool>(doc, "negate");
182184
}
183185

184-
std::cout << "[DEBUG] [map_io]: resolution: " << load_parameters.resolution << std::endl;
185-
std::cout << "[DEBUG] [map_io]: origin[0]: " << load_parameters.origin[0] << std::endl;
186-
std::cout << "[DEBUG] [map_io]: origin[1]: " << load_parameters.origin[1] << std::endl;
187-
std::cout << "[DEBUG] [map_io]: origin[2]: " << load_parameters.origin[2] << std::endl;
188-
std::cout << "[DEBUG] [map_io]: free_thresh: " << load_parameters.free_thresh << std::endl;
189-
std::cout << "[DEBUG] [map_io]: occupied_thresh: " << load_parameters.occupied_thresh <<
190-
std::endl;
191-
std::cout << "[DEBUG] [map_io]: mode: " << map_mode_to_string(load_parameters.mode) << std::endl;
192-
std::cout << "[DEBUG] [map_io]: negate: " << load_parameters.negate << std::endl; //NOLINT
186+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "resolution: " << load_parameters.resolution);
187+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[0]: " << load_parameters.origin[0]);
188+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[1]: " << load_parameters.origin[1]);
189+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "origin[2]: " << load_parameters.origin[2]);
190+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "free_thresh: " << load_parameters.free_thresh);
191+
RCLCPP_INFO_STREAM(
192+
rclcpp::get_logger(
193+
"map_io"), "occupied_thresh: " << load_parameters.occupied_thresh);
194+
RCLCPP_INFO_STREAM(
195+
rclcpp::get_logger("map_io"),
196+
"mode: " << map_mode_to_string(load_parameters.mode));
197+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "negate: " << load_parameters.negate);
193198

194199
return load_parameters;
195200
}
@@ -201,8 +206,9 @@ void loadMapFromFile(
201206
Magick::InitializeMagick(nullptr);
202207
nav_msgs::msg::OccupancyGrid msg;
203208

204-
std::cout << "[INFO] [map_io]: Loading image_file: " <<
205-
load_parameters.image_file_name << std::endl;
209+
RCLCPP_INFO_STREAM(
210+
rclcpp::get_logger("map_io"), "Loading image_file: " <<
211+
load_parameters.image_file_name);
206212
Magick::Image img(load_parameters.image_file_name);
207213

208214
// Copy the image data into the map structure
@@ -290,9 +296,11 @@ void loadMapFromFile(
290296
msg.header.frame_id = "map";
291297
msg.header.stamp = clock.now();
292298

293-
std::cout <<
294-
"[DEBUG] [map_io]: Read map " << load_parameters.image_file_name << ": " << msg.info.width <<
295-
" X " << msg.info.height << " map @ " << msg.info.resolution << " m/cell" << std::endl;
299+
RCLCPP_INFO_STREAM(
300+
rclcpp::get_logger(
301+
"map_io"), "Read map " << load_parameters.image_file_name
302+
<< ": " << msg.info.width << " X " << msg.info.height << " map @ "
303+
<< msg.info.resolution << " m/cell");
296304

297305
map = msg;
298306
}
@@ -302,30 +310,32 @@ LOAD_MAP_STATUS loadMapFromYaml(
302310
nav_msgs::msg::OccupancyGrid & map)
303311
{
304312
if (yaml_file.empty()) {
305-
std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl;
313+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!");
306314
return MAP_DOES_NOT_EXIST;
307315
}
308-
std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl;
316+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file);
309317
LoadParameters load_parameters;
310318
try {
311319
load_parameters = loadMapYaml(yaml_file);
312320
} catch (YAML::Exception & e) {
313-
std::cerr <<
314-
"[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" <<
315-
e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl;
321+
RCLCPP_ERROR_STREAM(
322+
rclcpp::get_logger(
323+
"map_io"), "Failed processing YAML file " << yaml_file << " at position (" <<
324+
e.mark.line << ":" << e.mark.column << ") for reason: " << e.what());
316325
return INVALID_MAP_METADATA;
317326
} catch (std::exception & e) {
318-
std::cerr <<
319-
"[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file <<
320-
" for reason: " << e.what() << std::endl;
327+
RCLCPP_ERROR_STREAM(
328+
rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file <<
329+
" for reason: " << e.what());
321330
return INVALID_MAP_METADATA;
322331
}
323332
try {
324333
loadMapFromFile(load_parameters, map);
325334
} catch (std::exception & e) {
326-
std::cerr <<
327-
"[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name <<
328-
" for reason: " << e.what() << std::endl;
335+
RCLCPP_ERROR_STREAM(
336+
rclcpp::get_logger(
337+
"map_io"), "Failed to load image file " << load_parameters.image_file_name <<
338+
" for reason: " << e.what());
329339
return INVALID_MAP_DATA;
330340
}
331341

@@ -350,40 +360,46 @@ void checkSaveParameters(SaveParameters & save_parameters)
350360
rclcpp::Clock clock(RCL_SYSTEM_TIME);
351361
save_parameters.map_file_name = "map_" +
352362
std::to_string(static_cast<int>(clock.now().seconds()));
353-
std::cout << "[WARN] [map_io]: Map file unspecified. Map will be saved to " <<
354-
save_parameters.map_file_name << " file" << std::endl;
363+
RCLCPP_WARN_STREAM(
364+
rclcpp::get_logger("map_io"), "Map file unspecified. Map will be saved to " <<
365+
save_parameters.map_file_name << " file");
355366
}
356367

357368
// Checking thresholds
358369
if (save_parameters.occupied_thresh == 0.0) {
359370
save_parameters.occupied_thresh = 0.65;
360-
std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " <<
361-
save_parameters.occupied_thresh << std::endl;
371+
RCLCPP_WARN_STREAM(
372+
rclcpp::get_logger(
373+
"map_io"), "Occupied threshold unspecified. Setting it to default value: " <<
374+
save_parameters.occupied_thresh);
362375
}
363376
if (save_parameters.free_thresh == 0.0) {
364377
save_parameters.free_thresh = 0.25;
365-
std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " <<
366-
save_parameters.free_thresh << std::endl;
378+
RCLCPP_WARN_STREAM(
379+
rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " <<
380+
save_parameters.free_thresh);
367381
}
368382
if (1.0 < save_parameters.occupied_thresh) {
369-
std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl;
383+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less");
370384
throw std::runtime_error("Incorrect thresholds");
371385
}
372386
if (save_parameters.free_thresh < 0.0) {
373-
std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl;
387+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater");
374388
throw std::runtime_error("Incorrect thresholds");
375389
}
376390
if (save_parameters.occupied_thresh <= save_parameters.free_thresh) {
377-
std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" <<
378-
std::endl;
391+
RCLCPP_ERROR_STREAM(
392+
rclcpp::get_logger(
393+
"map_io"), "Threshold_free must be smaller than threshold_occupied");
379394
throw std::runtime_error("Incorrect thresholds");
380395
}
381396

382397
// Checking image format
383398
if (save_parameters.image_format == "") {
384399
save_parameters.image_format = save_parameters.mode == MapMode::Scale ? "png" : "pgm";
385-
std::cout << "[WARN] [map_io]: Image format unspecified. Setting it to: " <<
386-
save_parameters.image_format << std::endl;
400+
RCLCPP_WARN_STREAM(
401+
rclcpp::get_logger("map_io"), "Image format unspecified. Setting it to: " <<
402+
save_parameters.image_format);
387403
}
388404

389405
std::transform(
@@ -406,24 +422,25 @@ void checkSaveParameters(SaveParameters & save_parameters)
406422
ss << "'" << format_name << "'";
407423
first = false;
408424
}
409-
std::cout <<
410-
"[WARN] [map_io]: Requested image format '" << save_parameters.image_format <<
411-
"' is not one of the recommended formats: " << ss.str() << std::endl;
425+
RCLCPP_WARN_STREAM(
426+
rclcpp::get_logger("map_io"), "Requested image format '" << save_parameters.image_format <<
427+
"' is not one of the recommended formats: " << ss.str());
412428
}
413429
const std::string FALLBACK_FORMAT = "png";
414430

415431
try {
416432
Magick::CoderInfo info(save_parameters.image_format);
417433
if (!info.isWritable()) {
418-
std::cout <<
419-
"[WARN] [map_io]: Format '" << save_parameters.image_format <<
420-
"' is not writable. Using '" << FALLBACK_FORMAT << "' instead" << std::endl;
434+
RCLCPP_WARN_STREAM(
435+
rclcpp::get_logger("map_io"), "Format '" << save_parameters.image_format <<
436+
"' is not writable. Using '" << FALLBACK_FORMAT << "' instead");
421437
save_parameters.image_format = FALLBACK_FORMAT;
422438
}
423439
} catch (Magick::ErrorOption & e) {
424-
std::cout <<
425-
"[WARN] [map_io]: Format '" << save_parameters.image_format << "' is not usable. Using '" <<
426-
FALLBACK_FORMAT << "' instead:" << std::endl << e.what() << std::endl;
440+
RCLCPP_WARN_STREAM(
441+
rclcpp::get_logger(
442+
"map_io"), "Format '" << save_parameters.image_format << "' is not usable. Using '" <<
443+
FALLBACK_FORMAT << "' instead:" << std::endl << e.what());
427444
save_parameters.image_format = FALLBACK_FORMAT;
428445
}
429446

@@ -434,10 +451,10 @@ void checkSaveParameters(SaveParameters & save_parameters)
434451
save_parameters.image_format == "jpg" ||
435452
save_parameters.image_format == "jpeg"))
436453
{
437-
std::cout <<
438-
"[WARN] [map_io]: Map mode 'scale' requires transparency, but format '" <<
439-
save_parameters.image_format <<
440-
"' does not support it. Consider switching image format to 'png'." << std::endl;
454+
RCLCPP_WARN_STREAM(
455+
rclcpp::get_logger("map_io"), "Map mode 'scale' requires transparency, but format '" <<
456+
save_parameters.image_format <<
457+
"' does not support it. Consider switching image format to 'png'.");
441458
}
442459
}
443460

@@ -451,9 +468,10 @@ void tryWriteMapToFile(
451468
const nav_msgs::msg::OccupancyGrid & map,
452469
const SaveParameters & save_parameters)
453470
{
454-
std::cout <<
455-
"[INFO] [map_io]: Received a " << map.info.width << " X " << map.info.height << " map @ " <<
456-
map.info.resolution << " m/pix" << std::endl;
471+
RCLCPP_INFO_STREAM(
472+
rclcpp::get_logger(
473+
"map_io"), "Received a " << map.info.width << " X " << map.info.height << " map @ " <<
474+
map.info.resolution << " m/pix");
457475

458476
std::string mapdatafile = save_parameters.map_file_name + "." + save_parameters.image_format;
459477
{
@@ -509,14 +527,18 @@ void tryWriteMapToFile(
509527
pixel = Magick::Color(q, q, q);
510528
break;
511529
default:
512-
std::cerr << "[ERROR] [map_io]: Map mode should be Trinary, Scale or Raw" << std::endl;
530+
RCLCPP_ERROR_STREAM(
531+
rclcpp::get_logger(
532+
"map_io"), "Map mode should be Trinary, Scale or Raw");
513533
throw std::runtime_error("Invalid map mode");
514534
}
515535
image.pixelColor(x, y, pixel);
516536
}
517537
}
518538

519-
std::cout << "[INFO] [map_io]: Writing map occupancy data to " << mapdatafile << std::endl;
539+
RCLCPP_INFO_STREAM(
540+
rclcpp::get_logger("map_io"),
541+
"Writing map occupancy data to " << mapdatafile);
520542
image.write(mapdatafile);
521543
}
522544

@@ -545,15 +567,15 @@ void tryWriteMapToFile(
545567
e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh;
546568

547569
if (!e.good()) {
548-
std::cout <<
549-
"[WARN] [map_io]: YAML writer failed with an error " << e.GetLastError() <<
550-
". The map metadata may be invalid." << std::endl;
570+
RCLCPP_ERROR_STREAM(
571+
rclcpp::get_logger("map_io"), "YAML writer failed with an error " << e.GetLastError() <<
572+
". The map metadata may be invalid.");
551573
}
552574

553-
std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl;
575+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile);
554576
std::ofstream(mapmetadatafile) << e.c_str();
555577
}
556-
std::cout << "[INFO] [map_io]: Map saved" << std::endl;
578+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved");
557579
}
558580

559581
bool saveMapToFile(
@@ -569,7 +591,9 @@ bool saveMapToFile(
569591

570592
tryWriteMapToFile(map, save_parameters_loc);
571593
} catch (std::exception & e) {
572-
std::cout << "[ERROR] [map_io]: Failed to write map for reason: " << e.what() << std::endl;
594+
RCLCPP_ERROR_STREAM(
595+
rclcpp::get_logger("map_io"),
596+
"Failed to write map for reason: " << e.what());
573597
return false;
574598
}
575599
return true;

0 commit comments

Comments
 (0)