Skip to content

Commit

Permalink
Distortion camera integration test (#1374)
Browse files Browse the repository at this point in the history
Signed-off-by: William Lew <[email protected]>
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Louise Poubel <[email protected]>
  • Loading branch information
3 people authored Mar 31, 2022
1 parent 4121a5c commit 2449e80
Show file tree
Hide file tree
Showing 3 changed files with 271 additions and 0 deletions.
1 change: 1 addition & 0 deletions test/integration/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
102 changes: 102 additions & 0 deletions test/integration/distortion_camera.cc
Original file line number Diff line number Diff line change
@@ -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 <gtest/gtest.h>

#ifdef _MSC_VER
#pragma warning(push, 0)
#endif
#include <ignition/msgs/image.pb.h>
#ifdef _MSC_VER
#pragma warning(pop)
#endif

#include "ignition/gazebo/Server.hh"
#include "ignition/gazebo/test_config.hh"
#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 "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;
}
168 changes: 168 additions & 0 deletions test/worlds/camera_distortion.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,168 @@
<?xml version="1.0" ?>
<sdf version="1.6">
<world name="lights">
<physics name="1ms" type="ode">
<max_step_size>0.001</max_step_size>
<real_time_factor>1.0</real_time_factor>
</physics>
<plugin
filename="ignition-gazebo-physics-system"
name="ignition::gazebo::systems::Physics">
</plugin>
<plugin
filename="ignition-gazebo-scene-broadcaster-system"
name="ignition::gazebo::systems::SceneBroadcaster">
</plugin>
<plugin
filename="ignition-gazebo-sensors-system"
name="ignition::gazebo::systems::Sensors">
<render_engine>ogre</render_engine>
<background_color>0 0 0 1</background_color>
</plugin>

<light type="directional" name="directional">
<cast_shadows>true</cast_shadows>
<diffuse>0.5 0.5 0.5 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<direction>0.0 0.0 -1.0</direction>
</light>

<model name="ground_plane">
<static>true</static>
<pose>3 0 -0.5 0 0 0</pose>
<link name="link">
<collision name="collision">
<geometry>
<plane>
<size>5 8</size>
</plane>
</geometry>
</collision>
<visual name="visual">
<geometry>
<plane>
<size>5 8</size>
</plane>
</geometry>
<material>
<ambient>0.5 0.5 0.5 1</ambient>
<diffuse>0.8 0.8 0.8 1</diffuse>
</material>
</visual>
</link>
</model>

<model name='camera_model_undistorted'>
<static>1</static>
<pose>0 0 0 0 0.0 0</pose>
<link name='body'>
<visual name='visual'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name='camera_sensor_undistorted' type='camera'>
<visualize>1</visualize>
<camera>
<horizontal_fov>0.927295218</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<topic>camera_sensor_undistorted</topic>
</sensor>
</link>
</model>

<model name='camera_model_barrel'>
<static>1</static>
<pose>0 0 0 0 0.0 0</pose>
<link name='body'>
<visual name='visual'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name='camera_sensor_barrel' type='camera'>
<visualize>1</visualize>
<camera>
<horizontal_fov>0.927295218</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<distortion>
<k1>-0.1349</k1>
<k2>-0.51868</k2>
<k3>-0.001</k3>
<p1>0</p1>
<p2>0</p2>
<center>0.5 0.5</center>
</distortion>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<topic>camera_sensor_barrel</topic>
</sensor>
</link>
</model>

<model name='camera_model_pincushion'>
<static>1</static>
<pose>0 0 0 0 0.0 0</pose>
<link name='body'>
<visual name='visual'>
<geometry>
<box>
<size>0.1 0.1 0.1</size>
</box>
</geometry>
</visual>
<sensor name='camera_sensor_pincushion' type='camera'>
<visualize>1</visualize>
<camera>
<horizontal_fov>0.927295218</horizontal_fov>
<image>
<width>320</width>
<height>240</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.1</near>
<far>100</far>
</clip>
<distortion>
<k1>0.1349</k1>
<k2>0.51868</k2>
<k3>0.001</k3>
<p1>0</p1>
<p2>0</p2>
<center>0.5 0.5</center>
</distortion>
</camera>
<always_on>1</always_on>
<update_rate>30</update_rate>
<topic>camera_sensor_pincushion</topic>
</sensor>
</link>
</model>

</world>
</sdf>

0 comments on commit 2449e80

Please sign in to comment.