From 885477b94ae4e27c499b96988e63339f2331b0a9 Mon Sep 17 00:00:00 2001
From: stevemacenski <stevenmacenski@gmail.com>
Date: Wed, 13 Nov 2019 06:27:10 -0800
Subject: [PATCH] fix clearing logic for near-max_obj_height instances
---
nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp | 5 +++++
nav2_costmap_2d/plugins/voxel_layer.cpp | 5 +++--
2 files changed, 8 insertions(+), 2 deletions(-)
diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
index 5e963c4f..764496cc 100644
--- a/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
+++ b/nav2_costmap_2d/include/nav2_costmap_2d/voxel_layer.hpp
@@ -152,6 +152,11 @@ private:
{
return sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0) + (z1 - z0) * (z1 - z0));
}
+
+ double getSizeInMetersZ() const
+ {
+ return (size_z_ - 1 + 0.5) * z_resolution_;
+ }
};
} // namespace nav2_costmap_2d
diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp
index 526b6200..24f2b6fa 100644
--- a/nav2_costmap_2d/plugins/voxel_layer.cpp
+++ b/nav2_costmap_2d/plugins/voxel_layer.cpp
@@ -314,6 +314,7 @@ void VoxelLayer::raytraceFreespace(
// we can pre-compute the enpoints of the map outside of the inner loop... we'll need these later
double map_end_x = origin_x_ + getSizeInMetersX();
double map_end_y = origin_y_ + getSizeInMetersY();
+ double map_end_z = origin_z_ + getSizeInMetersZ();
sensor_msgs::PointCloud2ConstIterator<float> iter_x(*(clearing_observation.cloud_), "x");
sensor_msgs::PointCloud2ConstIterator<float> iter_y(*(clearing_observation.cloud_), "y");
@@ -337,9 +338,9 @@ void VoxelLayer::raytraceFreespace(
double t = 1.0;
// we can only raytrace to a maximum z height
- if (wpz > max_obstacle_height_) {
+ if (wpz > map_end_z) {
// we know we want the vector's z value to be max_z
- t = std::max(0.0, std::min(t, (max_obstacle_height_ - 0.01 - oz) / c));
+ t = std::max(0.0, std::min(t, (map_end_z - 0.01 - oz) / c));
} else if (wpz < origin_z_) {
// and we can only raytrace down to the floor
// we know we want the vector's z value to be 0.0
--
GitLab