Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Distortion camera integration test #1374

Merged
merged 11 commits into from
Mar 31, 2022
Merged

Distortion camera integration test #1374

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
3b887a6
Added distortion world
WilliamLewww Jan 11, 2022
48835ce
Fix distortion camera world to use ogre1
WilliamLewww Jan 20, 2022
442cb75
Added integration test for distortion camera
WilliamLewww Mar 4, 2022
abdddc3
Merge branch 'ign-gazebo6' into wlew/distortion_test
WilliamLewww Mar 4, 2022
f2ac628
Fixed codecheck errors
WilliamLewww Mar 5, 2022
9e30ef8
Applied code suggestions
WilliamLewww Mar 7, 2022
23e7137
Merge branch 'ign-gazebo6' into wlew/distortion_test
ahcorde Mar 8, 2022
6bf6bc6
Merge branch 'ign-gazebo6' into wlew/distortion_test
WilliamLewww Mar 14, 2022
0f77f4a
Merge branch 'ign-gazebo6' into wlew/distortion_test
WilliamLewww Mar 16, 2022
7b7a3a0
Merge branch 'ign-gazebo6' into wlew/distortion_test
WilliamLewww Mar 29, 2022
8848a50
Merge branch 'ign-gazebo6' into wlew/distortion_test
chapulina Mar 30, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions test/integration/CMakeLists.txt
View file
Open in desktop
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
View file
Open in desktop
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>
WilliamLewww marked this conversation as resolved.
Show resolved Hide resolved
#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
View file
Open in desktop
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>