diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 5581a236..7e054470 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -177,7 +177,7 @@ foreach(plugin_test ${plugin_test_targets}) if(TARGET ${plugin_test}) set(_env_vars) list(APPEND _env_vars "IGN_PLUGIN_PATH=$") - list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") + list(APPEND _env_vars "DYLD_LIBRARY_PATH=$") set_tests_properties(${plugin_test} PROPERTIES ENVIRONMENT "${_env_vars}") diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index f40ab556..5429ab01 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -568,19 +568,18 @@ void CameraSensor::PopulateInfo(const sdf::Camera *_cameraSdf) intrinsics->add_k(0.0); intrinsics->add_k(1.0); - // TODO(anyone) Get tx and ty from SDF msgs::CameraInfo::Projection *proj = this->dataPtr->infoMsg.mutable_projection(); - proj->add_p(_cameraSdf->LensIntrinsicsFx()); + proj->add_p(_cameraSdf->LensProjectionFx()); proj->add_p(0.0); - proj->add_p(_cameraSdf->LensIntrinsicsCx()); - proj->add_p(-_cameraSdf->LensIntrinsicsFx() * this->dataPtr->baseline); + proj->add_p(_cameraSdf->LensProjectionCx()); + proj->add_p(_cameraSdf->LensProjectionTx()); proj->add_p(0.0); - proj->add_p(_cameraSdf->LensIntrinsicsFy()); - proj->add_p(_cameraSdf->LensIntrinsicsCy()); - proj->add_p(0.0); + proj->add_p(_cameraSdf->LensProjectionFy()); + proj->add_p(_cameraSdf->LensProjectionCy()); + proj->add_p(_cameraSdf->LensProjectionTy()); proj->add_p(0.0); proj->add_p(0.0); diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index e2fa1284..b148def7 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -107,6 +107,14 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " 124" << " 1.2" << " " + << " " + << " 282" + << " 283" + << " 163" + << " 125" + << " 1" + << " 2" + << " " << " " << " " << " " diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2c5de9fe..c0de2b96 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -59,7 +59,7 @@ foreach(plugin_test ${dri_tests} ${tests}) if(TARGET ${BINARY_NAME}) set(_env_vars) list(APPEND _env_vars "IGN_PLUGIN_PATH=$") - list(APPEND _env_vars "DYLD_LIBRARY_PATH=${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") + list(APPEND _env_vars "DYLD_LIBRARY_PATH=$") set_tests_properties(${BINARY_NAME} PROPERTIES ENVIRONMENT "${_env_vars}") diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index 8b8320a5..48adae79 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -18,8 +18,10 @@ #include #include +#include #include #include +#include // TODO(louise) Remove these pragmas once ign-rendering is disabling the // warnings @@ -48,6 +50,20 @@ #include "TransportTestTools.hh" +std::mutex g_infoMutex; +unsigned int g_infoCounter = 0; +ignition::msgs::CameraInfo g_infoMsg; + +////////////////////////////////////////////////// +void OnCameraInfo(const ignition::msgs::CameraInfo & _msg) +{ + g_infoMutex.lock(); + g_infoCounter++; + g_infoMsg.CopyFrom(_msg); + g_infoMutex.unlock(); +} + +////////////////////////////////////////////////// class CameraSensorTest: public testing::Test, public testing::WithParamInterface { @@ -55,6 +71,7 @@ class CameraSensorTest: public testing::Test, public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); }; +////////////////////////////////////////////////// void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) { // get the darn test data @@ -97,6 +114,12 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) EXPECT_EQ(std::string("base_camera"), sensor->FrameId()); + // subscribe to the camera info topic + std::string infoTopic = sensor->InfoTopic(); + ignition::transport::Node node; + node.Subscribe(infoTopic, &OnCameraInfo); + WaitForMessageTestHelper helperInfo(infoTopic); + std::string topic = "/test/integration/CameraPlugin_imagesWithBuiltinSDF"; WaitForMessageTestHelper helper(topic); @@ -104,6 +127,45 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) mgr.RunOnce(ignition::common::Time::Zero); EXPECT_TRUE(helper.WaitForMessage()) << helper; + EXPECT_TRUE(helperInfo.WaitForMessage()) << helperInfo; + + // Check CameraInfo properties + ignition::msgs::CameraInfo infoMsg; + g_infoMutex.lock(); + EXPECT_EQ(1u, g_infoCounter); + infoMsg.CopyFrom(g_infoMsg); + g_infoMutex.unlock(); + + { + auto intrinsics = infoMsg.intrinsics(); + EXPECT_EQ(9, intrinsics.k_size()); + EXPECT_DOUBLE_EQ(280.0, intrinsics.k(0)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(1)); + EXPECT_DOUBLE_EQ(162.0, intrinsics.k(2)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(3)); + EXPECT_DOUBLE_EQ(281.0, intrinsics.k(4)); + EXPECT_DOUBLE_EQ(124.0, intrinsics.k(5)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(6)); + EXPECT_DOUBLE_EQ(0.0, intrinsics.k(7)); + EXPECT_DOUBLE_EQ(1.0, intrinsics.k(8)); + } + + { + auto projection = infoMsg.projection(); + EXPECT_EQ(12, projection.p_size()); + EXPECT_DOUBLE_EQ(282.0, projection.p(0)); + EXPECT_DOUBLE_EQ(0.0, projection.p(1)); + EXPECT_DOUBLE_EQ(163.0, projection.p(2)); + EXPECT_DOUBLE_EQ(1.0, projection.p(3)); + EXPECT_DOUBLE_EQ(0.0, projection.p(4)); + EXPECT_DOUBLE_EQ(283.0, projection.p(5)); + EXPECT_DOUBLE_EQ(125.0, projection.p(6)); + EXPECT_DOUBLE_EQ(2.0, projection.p(7)); + EXPECT_DOUBLE_EQ(0.0, projection.p(8)); + EXPECT_DOUBLE_EQ(0.0, projection.p(9)); + EXPECT_DOUBLE_EQ(1.0, projection.p(10)); + EXPECT_DOUBLE_EQ(0.0, projection.p(11)); + } // Clean up engine->DestroyScene(scene); diff --git a/test/integration/camera_sensor_builtin.sdf b/test/integration/camera_sensor_builtin.sdf index 532ed003..12662eba 100644 --- a/test/integration/camera_sensor_builtin.sdf +++ b/test/integration/camera_sensor_builtin.sdf @@ -21,6 +21,34 @@ 0.0 0.007 + + custom + false + + 1.1 + 2.2 + 3.3 + 1.2 + sin + + 0.7505 + 128 + + 280 + 281 + 162 + 124 + 1.2 + + + 282 + 283 + 163 + 125 + 1 + 2 + +