Fix costmap publisher test (#3679)

* added printouts

* ignore system tests

* fix

* cleanup

* Update test_costmap_2d_publisher.cpp

remove space

nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp

@@ -117,7 +117,7 @@ class LayerSubscriber
 protected:
   void layerCallback(const nav2_msgs::msg::Costmap::SharedPtr layer)
   {
-    if (!callback_hit_) {
+    if (!callback_hit_ && (layer->data.size() == 100)) {
       layer_promise_.set_value(layer);
       callback_hit_ = true;
     }
@@ -157,23 +157,24 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test)
 {
   auto future = layer_subscriber_->layer_promise_.get_future();
   auto status = future.wait_for(std::chrono::seconds(5));
-  EXPECT_TRUE(status == std::future_status::ready);
+  ASSERT_TRUE(status == std::future_status::ready);
 
   auto costmap_raw = future.get();
 
   // Check first 20 cells of the 10by10 map
+  ASSERT_TRUE(costmap_raw->data.size() == 100);
   unsigned int i = 0;
   for (; i < 7; ++i) {
-    EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE);
+    EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE);
   }
   for (; i < 10; ++i) {
-    EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::LETHAL_OBSTACLE);
+    EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE);
   }
   for (; i < 17; ++i) {
-    EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE);
+    EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE);
   }
   for (; i < 20; ++i) {
-    EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::LETHAL_OBSTACLE);
+    EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE);
   }
 
   SUCCEED();