Update obstacle layer usage of max ranges

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -523,8 +523,8 @@ 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_;
+    unsigned int cell_max_range = cellDistance(obs.obstacle_max_range_);
+    unsigned int cell_min_range = cellDistance(obs.obstacle_min_range_);
 
     sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
     sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
@@ -545,20 +545,27 @@ 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);
+      // compute the distance from the hitpoint to the pointcloud's origin
+      // Calculate the distance in cells to match the ray trace algorithm used for clearing
+      // obstacles (see Costmap2D::raytraceLine).
+      unsigned int origin_cell_x;
+      unsigned int origin_cell_y;
+      worldToMap(obs.origin_.x, obs.origin_.y, origin_cell_x, origin_cell_y);
+      unsigned int point_cell_x;
+      unsigned int point_cell_y;
+      worldToMap(px, py, point_cell_x, point_cell_y);
+      int delta_x = point_cell_x - origin_cell_x;
+      int delta_y = point_cell_y - origin_cell_y;
+      unsigned int max_delta = std::max(std::abs(delta_x), std::abs(delta_y));
 
       // if the point is far enough away... we won't consider it
-      if (sq_dist >= sq_obstacle_max_range) {
+      if (max_delta > cell_max_range) {
         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 (max_delta < cell_min_range) {
         RCLCPP_DEBUG(logger_, "The point is too close");
         continue;
       }
@@ -768,6 +775,17 @@ ObstacleLayer::raytraceFreespace(
 
     unsigned int cell_raytrace_max_range = cellDistance(clearing_observation.raytrace_max_range_);
     unsigned int cell_raytrace_min_range = cellDistance(clearing_observation.raytrace_min_range_);
+
+    // Calculate the distance to the endpoint in cells to limit the maximum length of the raytrace
+    // and avoid clearing the endpoint cell.
+    int delta_x = x1 - x0;
+    int delta_y = y1 - y0;
+    unsigned int cell_dist_to_endpoint = std::max(std::abs(delta_x), std::abs(delta_y));
+    if (cell_dist_to_endpoint < 1) {
+      continue;
+    }
+    cell_raytrace_max_range = std::min(cell_raytrace_max_range, cell_dist_to_endpoint - 1);
+
     MarkCell marker(costmap_, FREE_SPACE);
     // and finally... we can execute our trace to clear obstacles along that line
     nav2_util::raytraceLine(

nav2_costmap_2d/test/unit/CMakeLists.txt

@@ -67,3 +67,9 @@ ament_add_gtest(coordinate_transform_test coordinate_transform_test.cpp)
 target_link_libraries(coordinate_transform_test
   nav2_costmap_2d_core
 )
+
+ament_add_gtest(obstacle_layer_test obstacle_layer_test.cpp)
+target_link_libraries(obstacle_layer_test
+  nav2_costmap_2d_core
+  layers
+)