Better Costmap Error Message (#2884)

* Better Costmap Error Message

* PR Feedback

Author: DLu

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -571,8 +571,12 @@ 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, %.2f) to (%.2f, %.2f). "
+      "The costmap cannot raytrace for it.",
+      ox, oy,
+      origin_x_, origin_y_,
+      origin_x_ + getSizeInMetersX(), origin_y_ + getSizeInMetersY(),
+    );
     return;
   }
 

nav2_costmap_2d/plugins/voxel_layer.cpp

@@ -275,8 +275,12 @@ 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, %.2f, %.2f) to (%.2f, %.2f, %.2f). "
+      "The costmap cannot raytrace for it.",
+      ox, oy, oz,
+      ox + getSizeInMetersX(), oy + getSizeInMetersY(), oz + getSizeInMetersZ(),
+      origin_x_, origin_y_, origin_z_);
+
     return;
   }