From 6d79e5832ea9501f106c9b777e403c5a4c489c85 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 26 Oct 2022 15:45:42 +0800 Subject: [PATCH 1/7] Adds a basic unit test for PointCloud functionality This PR adds a very trivial (happy path) unit test for PointCloud functionality. Hopefully, I will get more time to add more functionality to this test over time. TODOS: * Check behaviour with NaNs * Check mismatched float and pointcloud sizes * Check malformed point cloud. Signed-off-by: Arjo Chakravarty --- test/integration/point_cloud.cc | 206 ++++++++++++++++++++++++++++++++ 1 file changed, 206 insertions(+) create mode 100644 test/integration/point_cloud.cc diff --git a/test/integration/point_cloud.cc b/test/integration/point_cloud.cc new file mode 100644 index 000000000..0d8ff111d --- /dev/null +++ b/test/integration/point_cloud.cc @@ -0,0 +1,206 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "test_config.hh" // NOLINT(build/include) +#include "gz/gui/Application.hh" +#include "gz/gui/GuiEvents.hh" +#include "gz/gui/MainWindow.hh" +#include "gz/gui/Plugin.hh" + +int g_argc = 1; +char* g_argv[] = +{ + reinterpret_cast(const_cast("./PointCloud_TEST")), +}; + +using namespace std::chrono_literals; + +using namespace gz; +using namespace gui; + +class PointCloudTestFixture : public ::testing::Test +{ + + public: + gz::transport::Node node; + rendering::ScenePtr scene; + gz::transport::Node::Publisher pointcloudPub; + gz::transport::Node::Publisher flatPub; + gz::msgs::PointCloudPacked pcMsg; + gz::msgs::Float_V flatMsg; + std::atomic receivedMsg; + + PointCloudTestFixture() + { + // Periodic world statistics + this->pointcloudPub = this->node.Advertise( + "/point_cloud"); + this->flatPub = this->node.Advertise("/flat"); + + this->InitMockData(); + + this->node.Advertise("/marker", + &PointCloudTestFixture::OnMarkerMsg, this); + this->receivedMsg = false; + } + + public: void InitMockData() + { + + gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, + {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); + + int numberOfPoints{1000}; + unsigned int dataSize{numberOfPoints * pcMsg.point_step()}; + pcMsg.mutable_data()->resize(dataSize); + pcMsg.set_height(1); + pcMsg.set_width(1000); + + // Populate messages + gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); + gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); + gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); + + for (float x = 0.0, y = 0.0, z = 0.0; + xIter != xIter.End(); + ++xIter, ++yIter, ++zIter) + { + *xIter = x; + *yIter = y; + *zIter = z; + flatMsg.add_data(x); + + x += 1.0; + if (x > 9) + { + x = 0.0; + y += 1.0; + } + if (y > 9) + { + y = 0.0; + z += 1.0; + } + } + } + + public: void Publish() + { + this->pointcloudPub.Publish(pcMsg); + this->flatPub.Publish(flatMsg); + } + + /// \brief Callback that receives marker messages. + /// \param[in] _req The marker message. + public: void OnMarkerMsg(const gz::msgs::Marker &_req) + { + if (_req.action() == gz::msgs::Marker::DELETE_ALL) + { + // TODO(arjo129): Don't clear all markers only one marker. + } + else if (_req.action() == gz::msgs::Marker::ADD_MODIFY) + { + EXPECT_EQ(_req.id(), 1); + EXPECT_EQ(_req.ns(), "/point_cloud/flat"); + EXPECT_EQ(_req.type(), gz::msgs::Marker::POINTS); + EXPECT_EQ(_req.visibility(), gz::msgs::Marker::GUI); + if (_req.point().size() != 0) + { + // We might recieve empty packets as the sending process + // is asynchronuous + EXPECT_EQ(_req.point().size(), this->flatMsg.data().size()); + EXPECT_EQ(_req.materials().size(), this->flatMsg.data().size()); + this->receivedMsg = true; + } + } + else + { + EXPECT_TRUE(false); + } + } +}; + +///////////////////////////////////////////////// +TEST_F(PointCloudTestFixture, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PointCloudTestFixture)) +{ + common::Console::SetVerbosity(4); + + // Load the plugin + Application app(g_argc, g_argv); + app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + // Load plugin + const char *pluginStr = + "" + "/point_cloud" + "/flat" + ""; + + tinyxml2::XMLDocument pluginDoc; + EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); + + EXPECT_TRUE(app.LoadPlugin("PointCloud", + pluginDoc.FirstChildElement("plugin"))); + + // Get main window + auto window = app.findChild(); + ASSERT_NE(window, nullptr); + + // Get plugin + auto plugins = window->findChildren(); + EXPECT_EQ(plugins.size(), 1); + + // Show, but don't exec, so we don't block + window->QuickWindow()->show(); + + + this->Publish(); + + int sleep = 0; + int maxSleep = 30; + while (sleep < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + this->Publish(); + sleep++; + } + + EXPECT_TRUE(this->receivedMsg); + + // Cleanup + plugins.clear(); +} From 40e553da8565a08c8ee8efab74741d26bb04e807 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 26 Oct 2022 16:22:45 +0800 Subject: [PATCH 2/7] Address feedback Signed-off-by: Arjo Chakravarty --- test/integration/point_cloud.cc | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/test/integration/point_cloud.cc b/test/integration/point_cloud.cc index 0d8ff111d..a3fe7a777 100644 --- a/test/integration/point_cloud.cc +++ b/test/integration/point_cloud.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2022 Open Source Robotics Foundation * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,11 +18,11 @@ #include #include -#include +#include #include #include -#include #include +#include #include #include @@ -53,7 +53,6 @@ using namespace gui; class PointCloudTestFixture : public ::testing::Test { - public: gz::transport::Node node; rendering::ScenePtr scene; @@ -79,7 +78,6 @@ class PointCloudTestFixture : public ::testing::Test public: void InitMockData() { - gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); @@ -139,7 +137,7 @@ class PointCloudTestFixture : public ::testing::Test EXPECT_EQ(_req.visibility(), gz::msgs::Marker::GUI); if (_req.point().size() != 0) { - // We might recieve empty packets as the sending process + // We might receive empty packets as the sending process // is asynchronuous EXPECT_EQ(_req.point().size(), this->flatMsg.data().size()); EXPECT_EQ(_req.materials().size(), this->flatMsg.data().size()); @@ -161,7 +159,8 @@ TEST_F(PointCloudTestFixture, // Load the plugin Application app(g_argc, g_argv); - app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + app.AddPluginPath( + common::joinPaths(std::string(PROJECT_BINARY_PATH), "lib")); // Load plugin const char *pluginStr = "" @@ -186,7 +185,6 @@ TEST_F(PointCloudTestFixture, // Show, but don't exec, so we don't block window->QuickWindow()->show(); - this->Publish(); int sleep = 0; From 9a1f57301acffca9107f842d5a7e97c8b7d5fe4d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 20 Dec 2022 21:13:07 +0800 Subject: [PATCH 3/7] Address PR feedback Signed-off-by: Arjo Chakravarty --- src/plugins/point_cloud/PointCloud_TEST.cc | 146 ++++++++++++++- test/integration/point_cloud.cc | 204 --------------------- 2 files changed, 142 insertions(+), 208 deletions(-) delete mode 100644 test/integration/point_cloud.cc diff --git a/src/plugins/point_cloud/PointCloud_TEST.cc b/src/plugins/point_cloud/PointCloud_TEST.cc index 5fee2ecf3..73aebb38c 100644 --- a/src/plugins/point_cloud/PointCloud_TEST.cc +++ b/src/plugins/point_cloud/PointCloud_TEST.cc @@ -18,15 +18,23 @@ #include #include +#include + #include #include +#include +#include +#include +#include +#include +#include #include #include "test_config.hh" // NOLINT(build/include) #include "gz/gui/Application.hh" +#include "gz/gui/GuiEvents.hh" #include "gz/gui/MainWindow.hh" #include "gz/gui/Plugin.hh" -#include "PointCloud.hh" int g_argc = 1; char* g_argv[] = @@ -34,19 +42,132 @@ char* g_argv[] = reinterpret_cast(const_cast("./PointCloud_TEST")), }; +using namespace std::chrono_literals; + using namespace gz; using namespace gui; +class PointCloudTestFixture : public ::testing::Test +{ + public: + transport::Node node; + rendering::ScenePtr scene; + transport::Node::Publisher pointcloudPub; + transport::Node::Publisher flatPub; + msgs::PointCloudPacked pcMsg; + msgs::Float_V flatMsg; + std::atomic receivedMsg; + + PointCloudTestFixture() + { + this->pointcloudPub = this->node.Advertise( + "/point_cloud"); + this->flatPub = this->node.Advertise("/flat"); + + this->InitMockData(); + + this->node.Advertise("/marker", + &PointCloudTestFixture::OnMarkerMsg, this); + this->receivedMsg = false; + } + + public: void InitMockData() + { + msgs::InitPointCloudPacked(this->pcMsg, "some_frame", true, + {{"xyz", msgs::PointCloudPacked::Field::FLOAT32}}); + + int numberOfPoints{1000}; + unsigned int dataSize{numberOfPoints * this->pcMsg.point_step()}; + this->pcMsg.mutable_data()->resize(dataSize); + this->pcMsg.set_height(1); + this->pcMsg.set_width(1000); + + // Populate messages + msgs::PointCloudPackedIterator xIter(this->pcMsg, "x"); + msgs::PointCloudPackedIterator yIter(this->pcMsg, "y"); + msgs::PointCloudPackedIterator zIter(this->pcMsg, "z"); + + for (float x = 0.0, y = 0.0, z = 0.0; + xIter != xIter.End(); + ++xIter, ++yIter, ++zIter) + { + *xIter = x; + *yIter = y; + *zIter = z; + flatMsg.add_data(x); + + x += 1.0; + if (x > 9) + { + x = 0.0; + y += 1.0; + } + if (y > 9) + { + y = 0.0; + z += 1.0; + } + } + } + + public: void Publish() + { + this->pointcloudPub.Publish(this->pcMsg); + this->flatPub.Publish(this->flatMsg); + } + + /// \brief Callback that receives marker messages. + /// \param[in] _req The marker message. + public: void OnMarkerMsg(const msgs::Marker &_req) + { + if (_req.action() == msgs::Marker::ADD_MODIFY) + { + EXPECT_EQ(_req.id(), 1); + EXPECT_EQ(_req.ns(), "/point_cloud/flat"); + EXPECT_EQ(_req.type(), msgs::Marker::POINTS); + EXPECT_EQ(_req.visibility(), msgs::Marker::GUI); + if (_req.point().size() != 0) + { + // We might receive empty packets as the sending process + // is asynchronuous + EXPECT_EQ(_req.point().size(), this->flatMsg.data().size()); + EXPECT_EQ(_req.materials().size(), this->flatMsg.data().size()); + this->receivedMsg = true; + } + } + else if(_req.action() == gz::msgs::Marker::DELETE_ALL) + { + // Do nothing. Its ok to clear the screen. + } + else + { + ADD_FAILURE(); + } + } +}; + ///////////////////////////////////////////////// -TEST(PointCloudTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PointCloud)) +TEST_F(PointCloudTestFixture, + GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PointCloudTestFixture)) { common::Console::SetVerbosity(4); // Load the plugin Application app(g_argc, g_argv); - app.AddPluginPath(std::string(PROJECT_BINARY_PATH) + "/lib"); + app.AddPluginPath( + common::joinPaths(std::string(PROJECT_BINARY_PATH), "lib")); + // Load plugin + const char *pluginStr = + "" + "/point_cloud" + "/flat" + ""; - EXPECT_TRUE(app.LoadPlugin("PointCloud")); + tinyxml2::XMLDocument pluginDoc; + EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); + + EXPECT_TRUE(app.LoadPlugin("PointCloud", + pluginDoc.FirstChildElement("plugin"))); // Get main window auto window = app.findChild(); @@ -56,6 +177,23 @@ TEST(PointCloudTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PointCloud)) auto plugins = window->findChildren(); EXPECT_EQ(plugins.size(), 1); + // Show, but don't exec, so we don't block + window->QuickWindow()->show(); + + this->Publish(); + + int sleep = 0; + int maxSleep = 30; + while (sleep < maxSleep && !this->receivedMsg) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + QCoreApplication::processEvents(); + this->Publish(); + sleep++; + } + + EXPECT_TRUE(this->receivedMsg); + // Cleanup plugins.clear(); } diff --git a/test/integration/point_cloud.cc b/test/integration/point_cloud.cc deleted file mode 100644 index a3fe7a777..000000000 --- a/test/integration/point_cloud.cc +++ /dev/null @@ -1,204 +0,0 @@ -/* - * Copyright (C) 2022 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * -*/ - -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "test_config.hh" // NOLINT(build/include) -#include "gz/gui/Application.hh" -#include "gz/gui/GuiEvents.hh" -#include "gz/gui/MainWindow.hh" -#include "gz/gui/Plugin.hh" - -int g_argc = 1; -char* g_argv[] = -{ - reinterpret_cast(const_cast("./PointCloud_TEST")), -}; - -using namespace std::chrono_literals; - -using namespace gz; -using namespace gui; - -class PointCloudTestFixture : public ::testing::Test -{ - public: - gz::transport::Node node; - rendering::ScenePtr scene; - gz::transport::Node::Publisher pointcloudPub; - gz::transport::Node::Publisher flatPub; - gz::msgs::PointCloudPacked pcMsg; - gz::msgs::Float_V flatMsg; - std::atomic receivedMsg; - - PointCloudTestFixture() - { - // Periodic world statistics - this->pointcloudPub = this->node.Advertise( - "/point_cloud"); - this->flatPub = this->node.Advertise("/flat"); - - this->InitMockData(); - - this->node.Advertise("/marker", - &PointCloudTestFixture::OnMarkerMsg, this); - this->receivedMsg = false; - } - - public: void InitMockData() - { - gz::msgs::InitPointCloudPacked(pcMsg, "some_frame", true, - {{"xyz", gz::msgs::PointCloudPacked::Field::FLOAT32}}); - - int numberOfPoints{1000}; - unsigned int dataSize{numberOfPoints * pcMsg.point_step()}; - pcMsg.mutable_data()->resize(dataSize); - pcMsg.set_height(1); - pcMsg.set_width(1000); - - // Populate messages - gz::msgs::PointCloudPackedIterator xIter(pcMsg, "x"); - gz::msgs::PointCloudPackedIterator yIter(pcMsg, "y"); - gz::msgs::PointCloudPackedIterator zIter(pcMsg, "z"); - - for (float x = 0.0, y = 0.0, z = 0.0; - xIter != xIter.End(); - ++xIter, ++yIter, ++zIter) - { - *xIter = x; - *yIter = y; - *zIter = z; - flatMsg.add_data(x); - - x += 1.0; - if (x > 9) - { - x = 0.0; - y += 1.0; - } - if (y > 9) - { - y = 0.0; - z += 1.0; - } - } - } - - public: void Publish() - { - this->pointcloudPub.Publish(pcMsg); - this->flatPub.Publish(flatMsg); - } - - /// \brief Callback that receives marker messages. - /// \param[in] _req The marker message. - public: void OnMarkerMsg(const gz::msgs::Marker &_req) - { - if (_req.action() == gz::msgs::Marker::DELETE_ALL) - { - // TODO(arjo129): Don't clear all markers only one marker. - } - else if (_req.action() == gz::msgs::Marker::ADD_MODIFY) - { - EXPECT_EQ(_req.id(), 1); - EXPECT_EQ(_req.ns(), "/point_cloud/flat"); - EXPECT_EQ(_req.type(), gz::msgs::Marker::POINTS); - EXPECT_EQ(_req.visibility(), gz::msgs::Marker::GUI); - if (_req.point().size() != 0) - { - // We might receive empty packets as the sending process - // is asynchronuous - EXPECT_EQ(_req.point().size(), this->flatMsg.data().size()); - EXPECT_EQ(_req.materials().size(), this->flatMsg.data().size()); - this->receivedMsg = true; - } - } - else - { - EXPECT_TRUE(false); - } - } -}; - -///////////////////////////////////////////////// -TEST_F(PointCloudTestFixture, - GZ_UTILS_TEST_ENABLED_ONLY_ON_LINUX(PointCloudTestFixture)) -{ - common::Console::SetVerbosity(4); - - // Load the plugin - Application app(g_argc, g_argv); - app.AddPluginPath( - common::joinPaths(std::string(PROJECT_BINARY_PATH), "lib")); - // Load plugin - const char *pluginStr = - "" - "/point_cloud" - "/flat" - ""; - - tinyxml2::XMLDocument pluginDoc; - EXPECT_EQ(tinyxml2::XML_SUCCESS, pluginDoc.Parse(pluginStr)); - - EXPECT_TRUE(app.LoadPlugin("PointCloud", - pluginDoc.FirstChildElement("plugin"))); - - // Get main window - auto window = app.findChild(); - ASSERT_NE(window, nullptr); - - // Get plugin - auto plugins = window->findChildren(); - EXPECT_EQ(plugins.size(), 1); - - // Show, but don't exec, so we don't block - window->QuickWindow()->show(); - - this->Publish(); - - int sleep = 0; - int maxSleep = 30; - while (sleep < maxSleep) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - QCoreApplication::processEvents(); - this->Publish(); - sleep++; - } - - EXPECT_TRUE(this->receivedMsg); - - // Cleanup - plugins.clear(); -} From 3e929bf746b8981e5bec8dd1e6378fd8800b73b8 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Tue, 20 Dec 2022 21:24:08 +0800 Subject: [PATCH 4/7] Style Signed-off-by: Arjo Chakravarty --- src/plugins/point_cloud/PointCloud_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/plugins/point_cloud/PointCloud_TEST.cc b/src/plugins/point_cloud/PointCloud_TEST.cc index 73aebb38c..446bbb4b4 100644 --- a/src/plugins/point_cloud/PointCloud_TEST.cc +++ b/src/plugins/point_cloud/PointCloud_TEST.cc @@ -19,11 +19,11 @@ #include #include +#include +#include #include #include -#include -#include #include #include #include From 81ea36ce077025b50549d4b6a48629180d6d677d Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 21 Dec 2022 08:46:06 +0800 Subject: [PATCH 5/7] Address PR Feedback Signed-off-by: Arjo Chakravarty --- src/plugins/point_cloud/PointCloud_TEST.cc | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/src/plugins/point_cloud/PointCloud_TEST.cc b/src/plugins/point_cloud/PointCloud_TEST.cc index 446bbb4b4..943ae0e64 100644 --- a/src/plugins/point_cloud/PointCloud_TEST.cc +++ b/src/plugins/point_cloud/PointCloud_TEST.cc @@ -24,15 +24,11 @@ #include #include -#include -#include -#include #include #include #include "test_config.hh" // NOLINT(build/include) #include "gz/gui/Application.hh" -#include "gz/gui/GuiEvents.hh" #include "gz/gui/MainWindow.hh" #include "gz/gui/Plugin.hh" @@ -42,8 +38,6 @@ char* g_argv[] = reinterpret_cast(const_cast("./PointCloud_TEST")), }; -using namespace std::chrono_literals; - using namespace gz; using namespace gui; @@ -51,7 +45,6 @@ class PointCloudTestFixture : public ::testing::Test { public: transport::Node node; - rendering::ScenePtr scene; transport::Node::Publisher pointcloudPub; transport::Node::Publisher flatPub; msgs::PointCloudPacked pcMsg; @@ -135,14 +128,12 @@ class PointCloudTestFixture : public ::testing::Test this->receivedMsg = true; } } - else if(_req.action() == gz::msgs::Marker::DELETE_ALL) - { - // Do nothing. Its ok to clear the screen. - } - else + else if (_req.action() != msgs::Marker::DELETE_ALL) { + // if DELETE_ALL, its ok to clear the screen. Otherwise fail ADD_FAILURE(); } + } }; @@ -184,14 +175,13 @@ TEST_F(PointCloudTestFixture, int sleep = 0; int maxSleep = 30; - while (sleep < maxSleep && !this->receivedMsg) + while (!this->receivedMsg && sleep < maxSleep) { + this->Publish(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); QCoreApplication::processEvents(); - this->Publish(); - sleep++; + ++sleep; } - EXPECT_TRUE(this->receivedMsg); // Cleanup From c1f232bf5eaf8ab3b7865eeb809cb9899905b614 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Thu, 22 Dec 2022 11:35:36 +0800 Subject: [PATCH 6/7] Test actual values Signed-off-by: Arjo Chakravarty --- src/plugins/point_cloud/PointCloud_TEST.cc | 46 ++++++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/src/plugins/point_cloud/PointCloud_TEST.cc b/src/plugins/point_cloud/PointCloud_TEST.cc index 943ae0e64..2b840d8aa 100644 --- a/src/plugins/point_cloud/PointCloud_TEST.cc +++ b/src/plugins/point_cloud/PointCloud_TEST.cc @@ -103,12 +103,21 @@ class PointCloudTestFixture : public ::testing::Test } } + /// \brief Publish pointcloud packed data public: void Publish() { this->pointcloudPub.Publish(this->pcMsg); this->flatPub.Publish(this->flatMsg); } + /// \brief Color for minimum value + public: const math::Color minColor{1.0f, 0.0f, 0.0f, 1.0f}; + + /// \brief Color for maximum value + public: const math::Color maxColor{0.0f, 1.0f, 0.0f, 1.0f}; + + public: bool indexRecieved[10][10][10] = {false}; + /// \brief Callback that receives marker messages. /// \param[in] _req The marker message. public: void OnMarkerMsg(const msgs::Marker &_req) @@ -125,6 +134,43 @@ class PointCloudTestFixture : public ::testing::Test // is asynchronuous EXPECT_EQ(_req.point().size(), this->flatMsg.data().size()); EXPECT_EQ(_req.materials().size(), this->flatMsg.data().size()); + + auto dR = maxColor.R() - minColor.R(); + auto dG = maxColor.G() - minColor.G(); + auto dB = maxColor.B() - minColor.B(); + auto dA = maxColor.A() - minColor.A(); + + for (std::size_t idx = 0; idx < _req.point().size(); idx++) + { + // Check color correctness + EXPECT_NEAR(dR * (_req.point()[idx].x() / 9) + minColor.R(), + _req.materials()[idx].diffuse().r(), 1e-3); + EXPECT_NEAR(dG * (_req.point()[idx].x() / 9) + minColor.G(), + _req.materials()[idx].diffuse().g(), 1e-3); + EXPECT_NEAR(dB * (_req.point()[idx].x() / 9) + minColor.B(), + _req.materials()[idx].diffuse().b(), 1e-3); + EXPECT_NEAR(dA * (_req.point()[idx].x() / 9) + minColor.A(), + _req.materials()[idx].diffuse().a(), 1e-3); + + std::size_t x = round(_req.point()[idx].x()); + std::size_t y = round(_req.point()[idx].y()); + std::size_t z = round(_req.point()[idx].z()); + + // Mark this voxel as occupied + this->indexRecieved[x][y][z] = true; + } + + // Check all points in the point cloud have been populated. + for (std::size_t i = 0; i < 10; i++) + { + for (std::size_t j = 0; j < 10; j++) + { + for (std::size_t k = 0; k < 10; k++) + { + EXPECT_TRUE(this->indexRecieved[i][j][k]); + } + } + } this->receivedMsg = true; } } From 9c5e3b6c7d23cfb55663017b611b9b24ba35b507 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 28 Aug 2023 20:56:23 +0000 Subject: [PATCH 7/7] fix warning Signed-off-by: Ian Chen --- src/plugins/point_cloud/PointCloud_TEST.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/plugins/point_cloud/PointCloud_TEST.cc b/src/plugins/point_cloud/PointCloud_TEST.cc index 2b840d8aa..f06072683 100644 --- a/src/plugins/point_cloud/PointCloud_TEST.cc +++ b/src/plugins/point_cloud/PointCloud_TEST.cc @@ -140,7 +140,7 @@ class PointCloudTestFixture : public ::testing::Test auto dB = maxColor.B() - minColor.B(); auto dA = maxColor.A() - minColor.A(); - for (std::size_t idx = 0; idx < _req.point().size(); idx++) + for (int idx = 0; idx < _req.point().size(); idx++) { // Check color correctness EXPECT_NEAR(dR * (_req.point()[idx].x() / 9) + minColor.R(),