From f49a2cab1f8f0d955fcdf0f1aaee955670df284f Mon Sep 17 00:00:00 2001 From: Atsushi Watanabe Date: Mon, 23 Apr 2018 14:38:35 +0900 Subject: [PATCH] costmap_cspace: clear update region on output layer --- costmap_cspace/include/costmap_3d_layer/output.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/costmap_cspace/include/costmap_3d_layer/output.h b/costmap_cspace/include/costmap_3d_layer/output.h index b74fe47c..56cfc540 100644 --- a/costmap_cspace/include/costmap_3d_layer/output.h +++ b/costmap_cspace/include/costmap_3d_layer/output.h @@ -115,6 +115,7 @@ class Costmap3dLayerOutput : public Costmap3dLayerBase UpdatedRegion region_merged = region; region_merged.merge(region_prev_); region_prev_ = region; + region_ = UpdatedRegion(); update_msg->x = region_merged.x_; update_msg->y = region_merged.y_; @@ -132,7 +133,7 @@ class Costmap3dLayerOutput : public Costmap3dLayerBase { const int x2 = update_msg->x + i; const int y2 = update_msg->y + j; - const int yaw2 = region_.yaw_ + k; + const int yaw2 = update_msg->yaw + k; const auto &m = map_->getCost(x2, y2, yaw2); const size_t addr = (k * update_msg->height + j) * update_msg->width + i;