costmap_cspace: publish empty costmap_update on out-of-bound (#743)

at-wat · Dec 24, 2024 · 72e0ca8 · 72e0ca8
1 parent 2702a47
commit 72e0ca8
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 10 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
@@ -10,8 +10,8 @@
  *     * Redistributions in binary form must reproduce the above copyright
  *       notice, this list of conditions and the following disclaimer in the
  *       documentation and/or other materials provided with the distribution.
- *     * Neither the name of the copyright holder nor the names of its 
- *       contributors may be used to endorse or promote products derived from 
+ *     * Neither the name of the copyright holder nor the names of its
+ *       contributors may be used to endorse or promote products derived from
  *       this software without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
@@ -120,17 +120,19 @@ class Costmap3dUpdateLayerOutput
     region_prev_ = region;
     region_ = UpdatedRegion();
 
-    if (region.width_ == 0 || region.height_ == 0)
-    {
-      return nullptr;
-    }
-
     update_msg->x = region_merged.x_;
     update_msg->y = region_merged.y_;
     update_msg->width = region_merged.width_;
     update_msg->height = region_merged.height_;
     update_msg->yaw = region_merged.yaw_;
     update_msg->angle = region_merged.angle_;
+
+    if (region.width_ == 0 || region.height_ == 0)
+    {
+      update_msg->width = update_msg->height = 0;
+      return update_msg;
+    }
+
     update_msg->data.resize(update_msg->width * update_msg->height * update_msg->angle);
     if ((update_msg->x == 0) && (update_msg->y == 0) && (update_msg->yaw == 0) &&
         (update_msg->width == map_->info.width) && (update_msg->height == map_->info.height) &&

diff --git a/costmap_cspace/src/costmap_3d.cpp b/costmap_cspace/src/costmap_3d.cpp
@@ -117,10 +117,16 @@ class Costmap3DOFNode
     {
       publishDebug(*map);
       pub_costmap_update_.publish(*update);
+      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.");
+      }
     }
     else
     {
-      ROS_WARN("Updated region of the costmap is empty. The position may be out-of-boundary, or input map is wrong.");
+      ROS_WARN_THROTTLE(5, "Failed to update the costmap.");
     }
     return true;
   }

diff --git a/costmap_cspace/test/src/test_costmap_3d.cpp b/costmap_cspace/test/src/test_costmap_3d.cpp
@@ -698,7 +698,9 @@ TEST(Costmap3dLayerOutput, CSpaceOutOfBoundary)
     }
     else
     {
-      EXPECT_FALSE(static_cast<bool>(updated)) << test_name;
+      ASSERT_TRUE(static_cast<bool>(updated)) << test_name;
+      EXPECT_EQ(0, updated->width) << test_name;
+      EXPECT_EQ(0, updated->height) << test_name;
     }
 
     // Second pass has only local updates
@@ -716,7 +718,9 @@ TEST(Costmap3dLayerOutput, CSpaceOutOfBoundary)
     }
     else
     {
-      EXPECT_FALSE(static_cast<bool>(updated)) << test_name;
+      ASSERT_TRUE(static_cast<bool>(updated)) << test_name;
+      EXPECT_EQ(0, updated->width) << test_name;
+      EXPECT_EQ(0, updated->height) << test_name;
     }
   }
 }

diff --git a/planner_cspace/test/src/test_planner_3d_map_size.cpp b/planner_cspace/test/src/test_planner_3d_map_size.cpp
@@ -175,6 +175,18 @@ TEST_F(Planner3DMapSize, OutOfRangeAll)
   ASSERT_TRUE(waitStatus(ros::Duration(2)));
 }
 
+TEST_F(Planner3DMapSize, ZeroSizeUpdate)
+{
+  const ros::Time now = ros::Time::now();
+  pub_map_.publish(generateCSpace3DMsg(now, 0x80, 0x80, 4));
+  ros::Duration(0.1).sleep();
+  pub_map_update_.publish(generateCSpace3DUpdateMsg(now, 1, 1, 0, 0, 0, 0));
+  ros::Duration(0.5).sleep();
+  ros::spinOnce();
+  cnt_status_ = 0;
+  ASSERT_TRUE(waitStatus(ros::Duration(2)));
+}
+
 TEST_F(Planner3DMapSize, IllOrderedUpdate)
 {
   const ros::Time now = ros::Time::now();