Fix logic

at-wat · Dec 24, 2024 · 8e9e3e1 · 8e9e3e1
1 parent 0781f10
commit 8e9e3e1
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
diff --git a/costmap_cspace/include/costmap_cspace/costmap_3d_layer/output.h b/costmap_cspace/include/costmap_cspace/costmap_3d_layer/output.h
@@ -127,8 +127,9 @@ class Costmap3dUpdateLayerOutput
     update_msg->yaw = region_merged.yaw_;
     update_msg->angle = region_merged.angle_;
 
-    if (update_msg->width_ * update_msg->height_ == 0)
+    if (region.width_ == 0 || region.height_ == 0)
     {
+      update_msg->width = update_msg->height = 0;
       return update_msg;
     }
 

diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp
@@ -117,9 +117,11 @@ class Costmap3DOFNode
     {
       publishDebug(*map);
       pub_costmap_update_.publish(*update);
-      if (update->width * update->height == 0)
+      if (update->width * update->height * update->angle == 0)
       {
-        ROS_WARN_THROTTLE(5, "Updated region of the costmap is empty. The position may be out-of-boundary, or input map is wrong.");
+        ROS_WARN_THROTTLE(
+            5, "Updated region of the costmap is empty. "
+               "The position may be out-of-boundary, or input map is wrong.");
       }
     }
     else