From 94b69b5beb358827d19be3d7e1dcb607a0c55425 Mon Sep 17 00:00:00 2001
From: William Lew <WilliamMilesLew@gmail.com>
Date: Fri, 4 Mar 2022 10:10:06 -0800
Subject: [PATCH] Added integration test for distortion camera

Signed-off-by: William Lew <WilliamMilesLew@gmail.com>
---
 test/integration/CMakeLists.txt       |  1 +
 test/integration/distortion_camera.cc | 96 +++++++++++++++++++++++++++
 2 files changed, 97 insertions(+)
 create mode 100644 test/integration/distortion_camera.cc

diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt
index 4a8bee70031..06ff3a074e9 100644
--- a/test/integration/CMakeLists.txt
+++ b/test/integration/CMakeLists.txt
@@ -77,6 +77,7 @@ set(tests_needing_display
   camera_sensor_background.cc
   camera_video_record_system.cc
   depth_camera.cc
+  distortion_camera.cc
   gpu_lidar.cc
   optical_tactile_plugin.cc
   rgbd_camera.cc
diff --git a/test/integration/distortion_camera.cc b/test/integration/distortion_camera.cc
new file mode 100644
index 00000000000..5b51618f1bf
--- /dev/null
+++ b/test/integration/distortion_camera.cc
@@ -0,0 +1,96 @@
+/*
+ * 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 <gtest/gtest.h>
+
+#include <ignition/msgs/image.pb.h>
+
+#include <ignition/common/Console.hh>
+#include <ignition/common/Util.hh>
+#include <ignition/math/Pose3.hh>
+#include <ignition/transport/Node.hh>
+#include <ignition/utilities/ExtraTestMacros.hh>
+
+#include "ignition/gazebo/Server.hh"
+#include "ignition/gazebo/test_config.hh"
+
+#include "plugins/MockSystem.hh"
+#include "../helpers/EnvTestFixture.hh"
+
+using namespace ignition;
+using namespace gazebo;
+using namespace std::chrono_literals;
+
+/// \brief Test WideAndleCameraTest system
+class DistortionCameraTest : public InternalFixture<::testing::Test>
+{
+};
+
+std::mutex mutex;
+msgs::Image imageMsg;
+unsigned char *imageBuffer = nullptr;
+
+/////////////////////////////////////////////////
+void imageCb(const msgs::Image &_msg)
+{
+  ASSERT_EQ(msgs::PixelFormatType::RGB_INT8,
+      _msg.pixel_format_type());
+
+  mutex.lock();
+  unsigned int imageSamples = _msg.width() * _msg.height() * 3;
+
+  if (!imageBuffer)
+    imageBuffer = new unsigned char[imageSamples];
+  memcpy(imageBuffer, _msg.data().c_str(), imageSamples);
+  mutex.unlock();
+}
+
+/////////////////////////////////////////////////
+// The test checks the Distortion Camera readings
+TEST_F(DistortionCameraTest, IGN_UTILS_TEST_DISABLED_ON_MAC(DistortionCameraBox))
+{
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "worlds", "camera_distortion.sdf");
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  // subscribe to the image topic
+  transport::Node node;
+  node.Subscribe("/camera_sensor_barrel", &imageCb);
+
+  // Run server and verify that we are receiving a message
+  // from the distortion camera
+  size_t iters100 = 100u;
+  server.Run(true, iters100, false);
+
+  int sleep{0};
+  int maxSleep{30};
+  while (imageBuffer == nullptr && sleep < maxSleep)
+  {
+    std::this_thread::sleep_for(100ms);
+    sleep++;
+  }
+  EXPECT_LT(sleep, maxSleep);
+  ASSERT_NE(imageBuffer, nullptr);
+
+  delete[] imageBuffer;
+}