Backport humble: Update obstacle layer usage of max range (#5775)

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -450,8 +450,14 @@ ObstacleLayer::updateBounds(
 
     const sensor_msgs::msg::PointCloud2 & cloud = *(obs.cloud_);
 
-    double sq_obstacle_max_range = obs.obstacle_max_range_ * obs.obstacle_max_range_;
-    double sq_obstacle_min_range = obs.obstacle_min_range_ * obs.obstacle_min_range_;
+    const unsigned int max_range_cells = cellDistance(obs.obstacle_max_range_);
+    const unsigned int min_range_cells = cellDistance(obs.obstacle_min_range_);
+
+    unsigned int x0, y0;
+    if (!worldToMap(obs.origin_.x, obs.origin_.y, x0, y0)) {
+      RCLCPP_DEBUG(logger_, "Sensor origin is out of map bounds");
+      continue;
+    }
 
     sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
     sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
@@ -472,31 +478,33 @@ ObstacleLayer::updateBounds(
         continue;
       }
 
-      // compute the squared distance from the hitpoint to the pointcloud's origin
-      double sq_dist =
-        (px -
-        obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) +
-        (pz - obs.origin_.z) * (pz - obs.origin_.z);
+      // now we need to compute the map coordinates for the observation
+      unsigned int mx, my;
+      if (!worldToMap(px, py, mx, my)) {
+        RCLCPP_DEBUG(logger_, "Computing map coords failed");
+        continue;
+      }
+
+      // compute the distance from the hitpoint to the pointcloud's origin
+      // Calculate the distance in cell space to match the ray trace algorithm
+      // used for clearing obstacles (see Costmap2D::raytraceLine).
+      const int dx = static_cast<int>(mx) - static_cast<int>(x0);
+      const int dy = static_cast<int>(my) - static_cast<int>(y0);
+      const unsigned int dist = static_cast<unsigned int>(
+        std::hypot(static_cast<double>(dx), static_cast<double>(dy)));
 
       // if the point is far enough away... we won't consider it
-      if (sq_dist >= sq_obstacle_max_range) {
+      if (dist > max_range_cells) {
         RCLCPP_DEBUG(logger_, "The point is too far away");
         continue;
       }
 
-      // if the point is too close, do not conisder it
-      if (sq_dist < sq_obstacle_min_range) {
+      // if the point is too close, do not consider it
+      if (dist < min_range_cells) {
         RCLCPP_DEBUG(logger_, "The point is too close");
         continue;
       }
 
-      // now we need to compute the map coordinates for the observation
-      unsigned int mx, my;
-      if (!worldToMap(px, py, mx, my)) {
-        RCLCPP_DEBUG(logger_, "Computing map coords failed");
-        continue;
-      }
-
       unsigned int index = getIndex(mx, my);
       costmap_[index] = LETHAL_OBSTACLE;
       touch(px, py, min_x, min_y, max_x, max_y);

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -56,3 +56,9 @@ target_link_libraries(denoise_layer_test
   ${PROJECT_NAME}::nav2_costmap_2d_core
   ${PROJECT_NAME}::layers
 )
+
+ament_add_gtest(obstacle_layer_test obstacle_layer_test.cpp)
+target_link_libraries(obstacle_layer_test
+  ${PROJECT_NAME}::nav2_costmap_2d_core
+  layers
+)