From eee6113364a9ce8975a8af62b745907745a3d7a3 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 2 Jul 2023 17:35:24 -0400 Subject: [PATCH 1/5] added printouts --- .../integration/test_costmap_2d_publisher.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 26463250a7..f3bc8be52f 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -15,6 +15,7 @@ #include #include +#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" @@ -156,24 +157,32 @@ class CostmapRosTestFixture : public ::testing::Test TEST_F(CostmapRosTestFixture, costmap_pub_test) { auto future = layer_subscriber_->layer_promise_.get_future(); + std::cout << "Get future" << std::endl; auto status = future.wait_for(std::chrono::seconds(5)); - EXPECT_TRUE(status == std::future_status::ready); + std::cout << "Waited for future" << std::endl; + ASSERT_TRUE(status == std::future_status::ready); auto costmap_raw = future.get(); + std::cout << "Got future" << std::endl; // Check first 20 cells of the 10by10 map + std::cout << costmap_raw->data.size() << std::endl; unsigned int i = 0; for (; i < 7; ++i) { - EXPECT_EQ(costmap_raw->data[i], nav2_costmap_2d::FREE_SPACE); + std::cout << "i: " << i << std::endl; + 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); + std::cout << "i: " << i << std::endl; + 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); + std::cout << "i: " << i << std::endl; + 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); + std::cout << "i: " << i << std::endl; + EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE); } SUCCEED(); From 3a2e4f30575722a35ac688ca609909c6a61d9928 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 2 Jul 2023 17:46:40 -0400 Subject: [PATCH 2/5] ignore system tests --- nav2_system_tests/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 nav2_system_tests/COLCON_IGNORE diff --git a/nav2_system_tests/COLCON_IGNORE b/nav2_system_tests/COLCON_IGNORE new file mode 100644 index 0000000000..e69de29bb2 From 117dd56cbd6776817c8cfeccd17a7b8aa4859465 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 2 Jul 2023 18:55:51 -0400 Subject: [PATCH 3/5] fix --- .../test/integration/test_costmap_2d_publisher.cpp | 12 ++---------- 1 file changed, 2 insertions(+), 10 deletions(-) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index f3bc8be52f..7ca71157d0 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -118,7 +118,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,31 +157,23 @@ class CostmapRosTestFixture : public ::testing::Test TEST_F(CostmapRosTestFixture, costmap_pub_test) { auto future = layer_subscriber_->layer_promise_.get_future(); - std::cout << "Get future" << std::endl; auto status = future.wait_for(std::chrono::seconds(5)); - std::cout << "Waited for future" << std::endl; ASSERT_TRUE(status == std::future_status::ready); auto costmap_raw = future.get(); - std::cout << "Got future" << std::endl; // Check first 20 cells of the 10by10 map - std::cout << costmap_raw->data.size() << std::endl; - unsigned int i = 0; + ASSERT_TRUE(costmap_raw->data.size() == 100); unsigned int i = 0; for (; i < 7; ++i) { - std::cout << "i: " << i << std::endl; EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); } for (; i < 10; ++i) { - std::cout << "i: " << i << std::endl; EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE); } for (; i < 17; ++i) { - std::cout << "i: " << i << std::endl; EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); } for (; i < 20; ++i) { - std::cout << "i: " << i << std::endl; EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::LETHAL_OBSTACLE); } From 179361f008a56ddbfb20161c53b324a4d15c1483 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Sun, 2 Jul 2023 20:25:12 -0400 Subject: [PATCH 4/5] cleanup --- .../test/integration/test_costmap_2d_publisher.cpp | 4 ++-- nav2_system_tests/COLCON_IGNORE | 0 2 files changed, 2 insertions(+), 2 deletions(-) delete mode 100644 nav2_system_tests/COLCON_IGNORE diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 7ca71157d0..d57612f614 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -15,7 +15,6 @@ #include #include -#include #include "nav2_costmap_2d/costmap_2d.hpp" #include "nav2_costmap_2d/costmap_subscriber.hpp" @@ -163,7 +162,8 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test) auto costmap_raw = future.get(); // Check first 20 cells of the 10by10 map - ASSERT_TRUE(costmap_raw->data.size() == 100); unsigned int i = 0; + ASSERT_TRUE(costmap_raw->data.size() == 100); + unsigned int i = 0; for (; i < 7; ++i) { EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE); } diff --git a/nav2_system_tests/COLCON_IGNORE b/nav2_system_tests/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 From 2669ef7e7887da2e769fd986c7f32551a9909b12 Mon Sep 17 00:00:00 2001 From: Joshua Wallace Date: Mon, 3 Jul 2023 12:38:09 -0400 Subject: [PATCH 5/5] Update test_costmap_2d_publisher.cpp remove space --- nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index d57612f614..790466f38b 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -162,7 +162,7 @@ TEST_F(CostmapRosTestFixture, costmap_pub_test) auto costmap_raw = future.get(); // Check first 20 cells of the 10by10 map - ASSERT_TRUE(costmap_raw->data.size() == 100); + ASSERT_TRUE(costmap_raw->data.size() == 100); unsigned int i = 0; for (; i < 7; ++i) { EXPECT_EQ(costmap_raw->data.at(i), nav2_costmap_2d::FREE_SPACE);