Skip to content

Commit fbd1d3e

Browse files
doisygGuillaume Doisy
doisyg
and
Guillaume Doisy
authored
[map_io] Replace std logs by rclcpp logs (#4720)
* replace std logs by rclcpp logs Signed-off-by: Guillaume Doisy <[email protected]> * RCLCPP_DEBUG to RCLCPP_INFO for visibility Signed-off-by: Guillaume Doisy <[email protected]> --------- Signed-off-by: Guillaume Doisy <[email protected]> Co-authored-by: Guillaume Doisy <[email protected]>
1 parent 4b41320 commit fbd1d3e

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
@@ -133,8 +133,10 @@ std::string expand_user_home_dir_if_needed(
133133
return yaml_filename;
134134
}
135135
if (home_variable_value.empty()) {
136-
std::cout << "[INFO] [map_io]: Map yaml file name starts with '~/' but no HOME variable set. \n"
137-
<< "[INFO] [map_io] User home dir will be not expanded \n";
136+
RCLCPP_INFO_STREAM(
137+
rclcpp::get_logger(
138+
"map_io"), "Map yaml file name starts with '~/' but no HOME variable set. \n"
139+
<< "[INFO] [map_io] User home dir will be not expanded \n");
138140
return yaml_filename;
139141
}
140142
const std::string prefix{home_variable_value};
@@ -182,15 +184,18 @@ LoadParameters loadMapYaml(const std::string & yaml_filename)
182184
load_parameters.negate = yaml_get_value<bool>(doc, "negate");
183185
}
184186

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

195200
return load_parameters;
196201
}
@@ -202,8 +207,9 @@ void loadMapFromFile(
202207
Magick::InitializeMagick(nullptr);
203208
nav_msgs::msg::OccupancyGrid msg;
204209

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

209215
// Copy the image data into the map structure
@@ -291,9 +297,11 @@ void loadMapFromFile(
291297
msg.header.frame_id = "map";
292298
msg.header.stamp = clock.now();
293299

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

298306
map = msg;
299307
}
@@ -303,30 +311,32 @@ LOAD_MAP_STATUS loadMapFromYaml(
303311
nav_msgs::msg::OccupancyGrid & map)
304312
{
305313
if (yaml_file.empty()) {
306-
std::cerr << "[ERROR] [map_io]: YAML file name is empty, can't load!" << std::endl;
314+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "YAML file name is empty, can't load!");
307315
return MAP_DOES_NOT_EXIST;
308316
}
309-
std::cout << "[INFO] [map_io]: Loading yaml file: " << yaml_file << std::endl;
317+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Loading yaml file: " << yaml_file);
310318
LoadParameters load_parameters;
311319
try {
312320
load_parameters = loadMapYaml(yaml_file);
313321
} catch (YAML::Exception & e) {
314-
std::cerr <<
315-
"[ERROR] [map_io]: Failed processing YAML file " << yaml_file << " at position (" <<
316-
e.mark.line << ":" << e.mark.column << ") for reason: " << e.what() << std::endl;
322+
RCLCPP_ERROR_STREAM(
323+
rclcpp::get_logger(
324+
"map_io"), "Failed processing YAML file " << yaml_file << " at position (" <<
325+
e.mark.line << ":" << e.mark.column << ") for reason: " << e.what());
317326
return INVALID_MAP_METADATA;
318327
} catch (std::exception & e) {
319-
std::cerr <<
320-
"[ERROR] [map_io]: Failed to parse map YAML loaded from file " << yaml_file <<
321-
" for reason: " << e.what() << std::endl;
328+
RCLCPP_ERROR_STREAM(
329+
rclcpp::get_logger("map_io"), "Failed to parse map YAML loaded from file " << yaml_file <<
330+
" for reason: " << e.what());
322331
return INVALID_MAP_METADATA;
323332
}
324333
try {
325334
loadMapFromFile(load_parameters, map);
326335
} catch (std::exception & e) {
327-
std::cerr <<
328-
"[ERROR] [map_io]: Failed to load image file " << load_parameters.image_file_name <<
329-
" for reason: " << e.what() << std::endl;
336+
RCLCPP_ERROR_STREAM(
337+
rclcpp::get_logger(
338+
"map_io"), "Failed to load image file " << load_parameters.image_file_name <<
339+
" for reason: " << e.what());
330340
return INVALID_MAP_DATA;
331341
}
332342

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

358369
// Checking thresholds
359370
if (save_parameters.occupied_thresh == 0.0) {
360371
save_parameters.occupied_thresh = 0.65;
361-
std::cout << "[WARN] [map_io]: Occupied threshold unspecified. Setting it to default value: " <<
362-
save_parameters.occupied_thresh << std::endl;
372+
RCLCPP_WARN_STREAM(
373+
rclcpp::get_logger(
374+
"map_io"), "Occupied threshold unspecified. Setting it to default value: " <<
375+
save_parameters.occupied_thresh);
363376
}
364377
if (save_parameters.free_thresh == 0.0) {
365378
save_parameters.free_thresh = 0.25;
366-
std::cout << "[WARN] [map_io]: Free threshold unspecified. Setting it to default value: " <<
367-
save_parameters.free_thresh << std::endl;
379+
RCLCPP_WARN_STREAM(
380+
rclcpp::get_logger("map_io"), "Free threshold unspecified. Setting it to default value: " <<
381+
save_parameters.free_thresh);
368382
}
369383
if (1.0 < save_parameters.occupied_thresh) {
370-
std::cerr << "[ERROR] [map_io]: Threshold_occupied must be 1.0 or less" << std::endl;
384+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Threshold_occupied must be 1.0 or less");
371385
throw std::runtime_error("Incorrect thresholds");
372386
}
373387
if (save_parameters.free_thresh < 0.0) {
374-
std::cerr << "[ERROR] [map_io]: Free threshold must be 0.0 or greater" << std::endl;
388+
RCLCPP_ERROR_STREAM(rclcpp::get_logger("map_io"), "Free threshold must be 0.0 or greater");
375389
throw std::runtime_error("Incorrect thresholds");
376390
}
377391
if (save_parameters.occupied_thresh <= save_parameters.free_thresh) {
378-
std::cerr << "[ERROR] [map_io]: Threshold_free must be smaller than threshold_occupied" <<
379-
std::endl;
392+
RCLCPP_ERROR_STREAM(
393+
rclcpp::get_logger(
394+
"map_io"), "Threshold_free must be smaller than threshold_occupied");
380395
throw std::runtime_error("Incorrect thresholds");
381396
}
382397

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

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

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

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

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

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

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

@@ -546,15 +568,15 @@ void tryWriteMapToFile(
546568
e << YAML::Key << "free_thresh" << YAML::Value << save_parameters.free_thresh;
547569

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

554-
std::cout << "[INFO] [map_io]: Writing map metadata to " << mapmetadatafile << std::endl;
576+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Writing map metadata to " << mapmetadatafile);
555577
std::ofstream(mapmetadatafile) << e.c_str();
556578
}
557-
std::cout << "[INFO] [map_io]: Map saved" << std::endl;
579+
RCLCPP_INFO_STREAM(rclcpp::get_logger("map_io"), "Map saved");
558580
}
559581

560582
bool saveMapToFile(
@@ -570,7 +592,9 @@ bool saveMapToFile(
570592

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

0 commit comments

Comments
 (0)