Update obstacle layer usage of max ranges (#5697)

* Use cell distance for obstacle marking max range

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Don't raytrace clear the cell containing the current observation

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Add tests for max marking and clearing ranges

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Fix cpplint failures

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Fix distance calculation

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* fix casting, formatting

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* check origin is in map, update CMakeLists

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* use hypot instead of squared dist

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Move origin calc out of loop

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Revert don't raytrace observation cell

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Revert don't raytrace origin if it is observation cell

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

* Remove new line

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

---------

Signed-off-by: Simon Dathan <simon.dathan@kudan.eu>

Author: sd-kudan

nav2_costmap_2d/plugins/obstacle_layer.cpp

@@ -527,8 +527,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");
@@ -549,31 +555,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

@@ -71,3 +71,9 @@ nav2_add_gtest(observation_buffer_test observation_buffer_test.cpp)
 target_link_libraries(observation_buffer_test
   nav2_costmap_2d_core
 )
+
+nav2_add_gtest(obstacle_layer_test obstacle_layer_test.cpp)
+target_link_libraries(obstacle_layer_test
+  nav2_costmap_2d_core
+  layers
+)