From b4d0ac0a04edc987be4cb019c6f0e7d2d5940bb2 Mon Sep 17 00:00:00 2001 From: "M. Mostafa Farzan" Date: Thu, 25 Nov 2021 02:32:59 +0330 Subject: [PATCH] Always initialize clearing endpoints in voxel layer --- nav2_costmap_2d/plugins/voxel_layer.cpp | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 51e70ab9d20..635f4babb00 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -333,13 +333,11 @@ void VoxelLayer::raytraceFreespace( publish_clearing_points = (node->count_subscribers("clearing_endpoints") > 0); } - if (publish_clearing_points) { - clearing_endpoints_->data.clear(); - clearing_endpoints_->width = clearing_observation.cloud_->width; - clearing_endpoints_->height = clearing_observation.cloud_->height; - clearing_endpoints_->is_dense = true; - clearing_endpoints_->is_bigendian = false; - } + clearing_endpoints_->data.clear(); + clearing_endpoints_->width = clearing_observation.cloud_->width; + clearing_endpoints_->height = clearing_observation.cloud_->height; + clearing_endpoints_->is_dense = true; + clearing_endpoints_->is_bigendian = false; sensor_msgs::PointCloud2Modifier modifier(*clearing_endpoints_); modifier.setPointCloud2Fields(