From e65741d7336b270e67a8b60f1e7fd7410f0a9f86 Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Fri, 1 Apr 2022 17:45:09 -0400 Subject: [PATCH 1/2] Better Costmap Error Message --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 7 +++++-- nav2_costmap_2d/plugins/voxel_layer.cpp | 7 +++++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 1ff6f86633f..992ea605866 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -571,8 +571,11 @@ ObstacleLayer::raytraceFreespace( if (!worldToMap(ox, oy, x0, y0)) { RCLCPP_WARN( logger_, - "Sensor origin at (%.2f, %.2f) is out of map bounds. The costmap cannot raytrace for it.", - ox, oy); + "Sensor origin at (%.2f, %.2f) is out of map bounds (%.2f x %.2f at %.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, + getSizeInMetersX(), getSizeInMetersX(), + origin_x_, origin_y_); return; } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index b0a1982601f..8dc8e2c0f5f 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -275,8 +275,11 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( logger_, - "Sensor origin: (%.2f, %.2f, %.2f), out of map bounds. The costmap can't raytrace for it.", - ox, oy, oz); + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds (%.2f x %.2f x %.2f at %.2f, %.2f, %.2f). " + "The costmap cannot raytrace for it.", + ox, oy, oz, + getSizeInMetersX(), getSizeInMetersX(), getSizeInMetersX(), + origin_x_, origin_y_, origin_z_); return; } From a23d97a652ca6195e4c574446fe9f7e94ba6c1fb Mon Sep 17 00:00:00 2001 From: "David V. Lu" Date: Mon, 4 Apr 2022 13:03:46 -0400 Subject: [PATCH 2/2] PR Feedback --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 7 ++++--- nav2_costmap_2d/plugins/voxel_layer.cpp | 5 +++-- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 992ea605866..9f736988d28 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -571,11 +571,12 @@ ObstacleLayer::raytraceFreespace( if (!worldToMap(ox, oy, x0, y0)) { RCLCPP_WARN( logger_, - "Sensor origin at (%.2f, %.2f) is out of map bounds (%.2f x %.2f at %.2f, %.2f). " + "Sensor origin at (%.2f, %.2f) is out of map bounds (%.2f, %.2f) to (%.2f, %.2f). " "The costmap cannot raytrace for it.", ox, oy, - getSizeInMetersX(), getSizeInMetersX(), - origin_x_, origin_y_); + origin_x_, origin_y_, + origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(), + ); return; } diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 8dc8e2c0f5f..6e3e17077a1 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -275,11 +275,12 @@ void VoxelLayer::raytraceFreespace( if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z)) { RCLCPP_WARN( logger_, - "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds (%.2f x %.2f x %.2f at %.2f, %.2f, %.2f). " + "Sensor origin at (%.2f, %.2f %.2f) is out of map bounds (%.2f, %.2f, %.2f) to (%.2f, %.2f, %.2f). " "The costmap cannot raytrace for it.", ox, oy, oz, - getSizeInMetersX(), getSizeInMetersX(), getSizeInMetersX(), + ox + getSizeInMetersX(), oy + getSizeInMetersY(), oz + getSizeInMetersZ(), origin_x_, origin_y_, origin_z_); + return; }