diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index ab9f4cec3a..df316988e7 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 0000000000..279e6d28e7 --- /dev/null +++ b/test/integration/distortion_camera.cc @@ -0,0 +1,102 @@ +/* + * 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 + +#ifdef _MSC_VER +#pragma warning(push, 0) +#endif +#include +#ifdef _MSC_VER +#pragma warning(pop) +#endif + +#include "ignition/gazebo/Server.hh" +#include "ignition/gazebo/test_config.hh" +#include +#include +#include +#include +#include + +#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; +} diff --git a/test/worlds/camera_distortion.sdf b/test/worlds/camera_distortion.sdf new file mode 100644 index 0000000000..dca73f8535 --- /dev/null +++ b/test/worlds/camera_distortion.sdf @@ -0,0 +1,168 @@ + + + + + 0.001 + 1.0 + + + + + + + ogre + 0 0 0 1 + + + + true + 0.5 0.5 0.5 1 + 0.5 0.5 0.5 1 + 0.0 0.0 -1.0 + + + + true + 3 0 -0.5 0 0 0 + + + + + 5 8 + + + + + + + 5 8 + + + + 0.5 0.5 0.5 1 + 0.8 0.8 0.8 1 + + + + + + + 1 + 0 0 0 0 0.0 0 + + + + + 0.1 0.1 0.1 + + + + + 1 + + 0.927295218 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + 1 + 30 + camera_sensor_undistorted + + + + + + 1 + 0 0 0 0 0.0 0 + + + + + 0.1 0.1 0.1 + + + + + 1 + + 0.927295218 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + -0.1349 + -0.51868 + -0.001 + 0 + 0 +
0.5 0.5
+ + + 1 + 30 + camera_sensor_barrel + + + + + + 1 + 0 0 0 0 0.0 0 + + + + + 0.1 0.1 0.1 + + + + + 1 + + 0.927295218 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + 0.1349 + 0.51868 + 0.001 + 0 + 0 +
0.5 0.5
+ + + 1 + 30 + camera_sensor_pincushion + + + + + +