From 3ace7ae5539829b506887de5b8188f29e95a8f81 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Sat, 10 Jun 2023 02:51:59 +0200 Subject: [PATCH 1/9] Support protobuf >= 22 (#351) Signed-off-by: Silvio Traversaro --- CMakeLists.txt | 2 -- 1 file changed, 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 041cae04..e9ccbffd 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,9 +38,7 @@ message(STATUS "\n\n-- ====== Finding Dependencies ======") #-------------------------------------- # Find Protobuf -set(REQ_PROTOBUF_VER 3) ign_find_package(IgnProtobuf - VERSION ${REQ_PROTOBUF_VER} REQUIRED PRETTY Protobuf) From 577b03e2f766df3e51403aca2eb0c4a9abb94496 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 1 Sep 2023 12:51:27 -0500 Subject: [PATCH 2/9] Prepare for 6.7.1 release (#376) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 23 +++++++++++++++++++++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 51817fc2..fdeab814 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-sensors6 VERSION 6.7.0) +project(ignition-sensors6 VERSION 6.7.1) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index af9314db..62a93037 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,28 @@ ## Gazebo Sensors 6 +### Gazebo Sensors 6.7.1 (2023-09-01) + +1. Support protobuf >= 22 + * [Pull request #351](https://github.com/gazebosim/gz-sensors/pull/351) + +1. Infrastructure + * [Pull request #335](https://github.com/gazebosim/gz-sensors/pull/335) + +1. Rename COPYING to LICENSE + * [Pull request #334](https://github.com/gazebosim/gz-sensors/pull/334) + +1. Fix links in Changelog + * [Pull request #330](https://github.com/gazebosim/gz-sensors/pull/330) + +1. Fix Camera info test + * [Pull request #326](https://github.com/gazebosim/gz-sensors/pull/326) + +1. clean up rendering resources + * [Pull request #324](https://github.com/gazebosim/gz-sensors/pull/324) + +1. Added Camera Info topic support for cameras + * [Pull request #285](https://github.com/gazebosim/gz-sensors/pull/285) + ### Gazebo Sensors 6.7.0 (2023-02-13) 1. Disable thermal camera test on MacOS. From 8741b9b05dd410e332e4f55730b3709c3bf0995f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 6 Oct 2023 02:46:15 +0200 Subject: [PATCH 3/9] [backport fortress] camera info topic published with the right data (#383) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --------- Signed-off-by: Alejandro Hernández Cordero Signed-off-by: Aditya Signed-off-by: Ian Chen Co-authored-by: Aditya Pande --- src/CameraSensor.cc | 218 +++++++++++- test/integration/camera.cc | 604 +++++++++++++++++++++++++++++++++ test/sdf/camera_intrinsics.sdf | 77 +++++ test/sdf/camera_projection.sdf | 70 ++++ 4 files changed, 966 insertions(+), 3 deletions(-) create mode 100644 test/sdf/camera_intrinsics.sdf create mode 100644 test/sdf/camera_projection.sdf diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index 313968a7..97b8810a 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -45,6 +45,8 @@ #include "gz/sensors/SensorFactory.hh" #include "gz/sensors/SensorTypes.hh" +#include + using namespace gz; using namespace sensors; @@ -67,6 +69,69 @@ class gz::sensors::CameraSensorPrivate public: bool SaveImage(const unsigned char *_data, unsigned int _width, unsigned int _height, common::Image::PixelFormatType _format); + /// \brief Computes the OpenGL NDC matrix + /// \param[in] _left Left vertical clipping plane + /// \param[in] _right Right vertical clipping plane + /// \param[in] _bottom Bottom horizontal clipping plane + /// \param[in] _top Top horizontal clipping plane + /// \param[in] _near Distance to the nearer depth clipping plane + /// This value is negative if the plane is to be behind + /// the camera + /// \param[in] _far Distance to the farther depth clipping plane + /// This value is negative if the plane is to be behind + /// the camera + /// \return OpenGL NDC (Normalized Device Coordinates) matrix + public: static math::Matrix4d BuildNDCMatrix( + double _left, double _right, + double _bottom, double _top, + double _near, double _far); + + /// \brief Computes the OpenGL perspective matrix + /// \param[in] _intrinsicsFx Horizontal focal length (in pixels) + /// \param[in] _intrinsicsFy Vertical focal length (in pixels) + /// \param[in] _intrinsicsCx X coordinate of principal point in pixels + /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels + /// \param[in] _intrinsicsS Skew coefficient defining the angle between + /// the x and y pixel axes + /// \param[in] _clipNear Distance to the nearer depth clipping plane + /// This value is negative if the plane is to be behind + /// the camera + /// \param[in] _clipFar Distance to the farther depth clipping plane + /// This value is negative if the plane is to be behind + /// the camera + /// \return OpenGL perspective matrix + public: static math::Matrix4d BuildPerspectiveMatrix( + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, + double _clipNear, double _clipFar); + + /// \brief Computes the OpenGL projection matrix by multiplying + /// the OpenGL Normalized Device Coordinates matrix (NDC) with + /// the OpenGL perspective matrix + /// openglProjectionMatrix = ndcMatrix * perspectiveMatrix + /// \param[in] _imageWidth Image width (in pixels) + /// \param[in] _imageHeight Image height (in pixels) + /// \param[in] _intrinsicsFx Horizontal focal length (in pixels) + /// \param[in] _intrinsicsFy Vertical focal length (in pixels) + /// \param[in] _intrinsicsCx X coordinate of principal point in pixels + /// \param[in] _intrinsicsCy Y coordinate of principal point in pixels + /// \param[in] _intrinsicsS Skew coefficient defining the angle between + /// the x and y pixel axes + /// \param[in] _clipNear Distance to the nearer depth clipping plane + /// This value is negative if the plane is to be behind + /// the camera + /// \param[in] _clipFar Distance to the farther depth clipping plane + /// This value is negative if the plane is to be behind + /// the camera + /// \return OpenGL projection matrix + public: static math::Matrix4d BuildProjectionMatrix( + double _imageWidth, double _imageHeight, + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, + double _clipNear, double _clipFar); + /// \brief node to create publisher public: transport::Node node; @@ -145,15 +210,13 @@ class gz::sensors::CameraSensorPrivate ////////////////////////////////////////////////// bool CameraSensor::CreateCamera() { - const sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); + sdf::Camera *cameraSdf = this->dataPtr->sdfSensor.CameraSensor(); if (!cameraSdf) { ignerr << "Unable to access camera SDF element.\n"; return false; } - this->PopulateInfo(cameraSdf); - unsigned int width = cameraSdf->ImageWidth(); unsigned int height = cameraSdf->ImageHeight(); @@ -229,6 +292,39 @@ bool CameraSensor::CreateCamera() break; } + // Update the DOM object intrinsics to have consistent + // intrinsics between ogre camera and camera_info msg + if(!cameraSdf->HasLensIntrinsics()) + { + auto intrinsicMatrix = + gz::rendering::projectionToCameraIntrinsic( + this->dataPtr->camera->ProjectionMatrix(), + this->dataPtr->camera->ImageWidth(), + this->dataPtr->camera->ImageHeight() + ); + + cameraSdf->SetLensIntrinsicsFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensIntrinsicsFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensIntrinsicsCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensIntrinsicsCy(intrinsicMatrix(1, 2)); + } + // set custom projection matrix based on intrinsics param specified in sdf + else + { + double fx = cameraSdf->LensIntrinsicsFx(); + double fy = cameraSdf->LensIntrinsicsFy(); + double cx = cameraSdf->LensIntrinsicsCx(); + double cy = cameraSdf->LensIntrinsicsCy(); + double s = cameraSdf->LensIntrinsicsSkew(); + auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + this->dataPtr->camera->ImageWidth(), + this->dataPtr->camera->ImageHeight(), + fx, fy, cx, cy, s, + this->dataPtr->camera->NearClipPlane(), + this->dataPtr->camera->FarClipPlane()); + this->dataPtr->camera->SetProjectionMatrix(projectionMatrix); + } + this->dataPtr->image = this->dataPtr->camera->CreateImage(); this->Scene()->RootVisual()->AddChild(this->dataPtr->camera); @@ -241,6 +337,51 @@ bool CameraSensor::CreateCamera() this->dataPtr->saveImage = true; } + // Update the DOM object intrinsics to have consistent + // projection matrix values between ogre camera and camera_info msg + // If these values are not defined in the SDF then we need to update + // these values to something reasonable. The projection matrix is + // the cumulative effect of intrinsic and extrinsic parameters + if(!cameraSdf->HasLensProjection()) + { + // Note that the matrix from Ogre via camera->ProjectionMatrix() has a + // different format than the projection matrix used in SDFormat. + // This is why they are converted using projectionToCameraIntrinsic. + // The resulting matrix is the intrinsic matrix, but since the user has + // not overridden the values, this is also equal to the projection matrix. + auto intrinsicMatrix = + gz::rendering::projectionToCameraIntrinsic( + this->dataPtr->camera->ProjectionMatrix(), + this->dataPtr->camera->ImageWidth(), + this->dataPtr->camera->ImageHeight() + ); + cameraSdf->SetLensProjectionFx(intrinsicMatrix(0, 0)); + cameraSdf->SetLensProjectionFy(intrinsicMatrix(1, 1)); + cameraSdf->SetLensProjectionCx(intrinsicMatrix(0, 2)); + cameraSdf->SetLensProjectionCy(intrinsicMatrix(1, 2)); + } + // set custom projection matrix based on projection param specified in sdf + else + { + // tx and ty are not used + double fx = cameraSdf->LensProjectionFx(); + double fy = cameraSdf->LensProjectionFy(); + double cx = cameraSdf->LensProjectionCx(); + double cy = cameraSdf->LensProjectionCy(); + double s = 0; + + auto projectionMatrix = CameraSensorPrivate::BuildProjectionMatrix( + this->dataPtr->camera->ImageWidth(), + this->dataPtr->camera->ImageHeight(), + fx, fy, cx, cy, s, + this->dataPtr->camera->NearClipPlane(), + this->dataPtr->camera->FarClipPlane()); + this->dataPtr->camera->SetProjectionMatrix(projectionMatrix); + } + + // Populate camera info topic + this->PopulateInfo(cameraSdf); + return true; } @@ -753,3 +894,74 @@ bool CameraSensor::HasInfoConnections() const { return this->dataPtr->infoPub && this->dataPtr->infoPub.HasConnections(); } + +////////////////////////////////////////////////// +math::Matrix4d CameraSensorPrivate::BuildProjectionMatrix( + double _imageWidth, double _imageHeight, + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, + double _clipNear, double _clipFar) +{ + return CameraSensorPrivate::BuildNDCMatrix( + 0, _imageWidth, 0, _imageHeight, _clipNear, _clipFar) * + CameraSensorPrivate::BuildPerspectiveMatrix( + _intrinsicsFx, _intrinsicsFy, + _intrinsicsCx, _imageHeight - _intrinsicsCy, + _intrinsicsS, _clipNear, _clipFar); +} + +////////////////////////////////////////////////// +math::Matrix4d CameraSensorPrivate::BuildNDCMatrix( + double _left, double _right, + double _bottom, double _top, + double _near, double _far) +{ + double inverseWidth = 1.0 / (_right - _left); + double inverseHeight = 1.0 / (_top - _bottom); + double inverseDistance = 1.0 / (_far - _near); + + return math::Matrix4d( + 2.0 * inverseWidth, + 0.0, + 0.0, + -(_right + _left) * inverseWidth, + 0.0, + 2.0 * inverseHeight, + 0.0, + -(_top + _bottom) * inverseHeight, + 0.0, + 0.0, + -2.0 * inverseDistance, + -(_far + _near) * inverseDistance, + 0.0, + 0.0, + 0.0, + 1.0); +} + +////////////////////////////////////////////////// +math::Matrix4d CameraSensorPrivate::BuildPerspectiveMatrix( + double _intrinsicsFx, double _intrinsicsFy, + double _intrinsicsCx, double _intrinsicsCy, + double _intrinsicsS, + double _clipNear, double _clipFar) +{ + return math::Matrix4d( + _intrinsicsFx, + _intrinsicsS, + -_intrinsicsCx, + 0.0, + 0.0, + _intrinsicsFy, + -_intrinsicsCy, + 0.0, + 0.0, + 0.0, + _clipNear + _clipFar, + _clipNear * _clipFar, + 0.0, + 0.0, + -1.0, + 0.0); +} diff --git a/test/integration/camera.cc b/test/integration/camera.cc index a5ef6e7d..ad5412db 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -31,6 +32,7 @@ #include #include #include +#include #ifdef _WIN32 #pragma warning(pop) #endif @@ -86,6 +88,12 @@ class CameraSensorTest: public testing::Test, // Create a 16 bit grayscale camera sensor and verify image format public: void ImageFormatLInt16(const std::string &_renderEngine); + + // Create camera sensors and verify camera intrinsics + public: void CameraIntrinsics(const std::string &_renderEngine); + + // Create camera sensors and verify camera projection + public: void CameraProjection(const std::string &_renderEngine); }; void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) @@ -180,6 +188,602 @@ TEST_P(CameraSensorTest, ImagesWithBuiltinSDF) ImagesWithBuiltinSDF(GetParam()); } +////////////////////////////////////////////////// +void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "camera_intrinsics.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + + // Camera sensor without intrinsics tag + auto cameraWithoutIntrinsicsTag = linkPtr->GetElement("sensor"); + + // Camera sensor with intrinsics tag + auto cameraWithIntrinsicsTag = + linkPtr->GetElement("sensor")->GetNextElement(); + + // Camera sensor with different intrinsics tag + auto cameraWithDiffIntrinsicsTag = + cameraWithIntrinsicsTag->GetNextElement(); + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + scene->SetAmbientLight(1.0, 1.0, 1.0); + + gz::rendering::VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // create blue material + gz::rendering::MaterialPtr blue = scene->CreateMaterial(); + blue->SetAmbient(0.0, 0.0, 0.3); + blue->SetDiffuse(0.0, 0.0, 0.8); + blue->SetSpecular(0.5, 0.5, 0.5); + + // create box visual + gz::rendering::VisualPtr box = scene->CreateVisual("box"); + ASSERT_NE(nullptr, box); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5)); + box->SetLocalRotation(0, 0, 0); + box->SetMaterial(blue); + scene->DestroyMaterial(blue); + root->AddChild(box); + + // Do the test + gz::sensors::Manager mgr; + + // there are 3 cameras: + // - camera1: no intrinsics params + // - camera2: has intrinsics params that are the same as default values + // - camera3: has intrinsics params that are different from default values + // camera1 and camera2 should produce very similar images and camera3 should + // produce different images from 1 and 2. + auto *sensor1 = + mgr.CreateSensor(cameraWithoutIntrinsicsTag); + auto *sensor2 = + mgr.CreateSensor(cameraWithIntrinsicsTag); + auto *sensor3 = + mgr.CreateSensor(cameraWithDiffIntrinsicsTag); + + ASSERT_NE(sensor1, nullptr); + ASSERT_NE(sensor2, nullptr); + ASSERT_NE(sensor3, nullptr); + sensor1->SetScene(scene); + sensor2->SetScene(scene); + sensor3->SetScene(scene); + + std::string infoTopic1 = "/camera1/camera_info"; + std::string infoTopic2 = "/camera2/camera_info"; + std::string infoTopic3 = "/camera3/camera_info"; + std::string imgTopic1 = "/camera1/image"; + std::string imgTopic2 = "/camera2/image"; + std::string imgTopic3 = "/camera3/image"; + WaitForMessageTestHelper helper1(imgTopic1); + WaitForMessageTestHelper helper2(infoTopic1); + WaitForMessageTestHelper helper3(imgTopic2); + WaitForMessageTestHelper helper4(infoTopic2); + WaitForMessageTestHelper helper5(imgTopic3); + WaitForMessageTestHelper helper6(infoTopic3); + + EXPECT_TRUE(sensor1->HasConnections()); + EXPECT_TRUE(sensor2->HasConnections()); + EXPECT_TRUE(sensor3->HasConnections()); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper1.WaitForMessage()) << helper1; + EXPECT_TRUE(helper2.WaitForMessage()) << helper2; + EXPECT_TRUE(helper3.WaitForMessage()) << helper3; + EXPECT_TRUE(helper4.WaitForMessage()) << helper4; + EXPECT_TRUE(helper5.WaitForMessage()) << helper5; + EXPECT_TRUE(helper6.WaitForMessage()) << helper6; + + // Subscribe to the camera info topic + gz::msgs::CameraInfo camera1Info, camera2Info, camera3Info; + unsigned int camera1Counter = 0u; + unsigned int camera2Counter = 0u; + unsigned int camera3Counter = 0u; + + std::function camera1InfoCallback = + [&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg) + { + camera1Info = _msg; + camera1Counter++; + }; + std::function camera2InfoCallback = + [&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg) + { + camera2Info = _msg; + camera2Counter++; + }; + std::function camera3InfoCallback = + [&camera3Info, &camera3Counter](const gz::msgs::CameraInfo& _msg) + { + camera3Info = _msg; + camera3Counter++; + }; + + unsigned int height = 1000u; + unsigned int width = 1000u; + unsigned int bpp = 3u; + unsigned int imgBufferSize = width * height * bpp; + unsigned char* img1 = new unsigned char[imgBufferSize]; + unsigned char* img2 = new unsigned char[imgBufferSize]; + unsigned char* img3 = new unsigned char[imgBufferSize]; + unsigned int camera1DataCounter = 0u; + unsigned int camera2DataCounter = 0u; + unsigned int camera3DataCounter = 0u; + std::function camera1Callback = + [&img1, &camera1DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img1, _msg.data().c_str(), imgBufferSize); + camera1DataCounter++; + }; + + std::function camera2Callback = + [&img2, &camera2DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img2, _msg.data().c_str(), imgBufferSize); + camera2DataCounter++; + }; + std::function camera3Callback = + [&img3, &camera3DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img3, _msg.data().c_str(), imgBufferSize); + camera3DataCounter++; + }; + + // Subscribe to the camera topic + gz::transport::Node node; + node.Subscribe(infoTopic1, camera1InfoCallback); + node.Subscribe(infoTopic2, camera2InfoCallback); + node.Subscribe(infoTopic3, camera3InfoCallback); + node.Subscribe(imgTopic1, camera1Callback); + node.Subscribe(imgTopic2, camera2Callback); + node.Subscribe(imgTopic3, camera3Callback); + + // Wait for a few camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + + // Run to get image and check image format in callback + bool done = false; + int sleep = 0; + int maxSleep = 10; + while (!done && sleep++ < maxSleep) + { + std::lock_guard lock(g_mutex); + done = (camera1Counter > 0u && camera2Counter > 0u && camera3Counter > 0u && + camera1DataCounter > 0u && camera2DataCounter > 0u && + camera3DataCounter > 0u); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Image size, focal length and optical center + // Camera sensor without intrinsics tag + double error = 3e-1; + EXPECT_EQ(camera1Info.width(), width); + EXPECT_EQ(camera1Info.height(), height); + EXPECT_NEAR(camera1Info.intrinsics().k(0), 866.23, error); + EXPECT_NEAR(camera1Info.intrinsics().k(4), 866.23, error); + EXPECT_DOUBLE_EQ(camera1Info.intrinsics().k(2), 500); + EXPECT_DOUBLE_EQ(camera1Info.intrinsics().k(5), 500); + + // Camera sensor with intrinsics tag + EXPECT_EQ(camera2Info.width(), width); + EXPECT_EQ(camera2Info.height(), height); + EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(0), 866.23); + EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(4), 866.23); + EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(2), 500); + EXPECT_DOUBLE_EQ(camera2Info.intrinsics().k(5), 500); + + // Camera sensor with different intrinsics tag + EXPECT_EQ(camera3Info.width(), width); + EXPECT_EQ(camera3Info.height(), height); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(0), 900); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(4), 900); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(2), 501); + EXPECT_DOUBLE_EQ(camera3Info.intrinsics().k(5), 501); + + unsigned int r1Sum = 0u; + unsigned int g1Sum = 0u; + unsigned int b1Sum = 0u; + unsigned int r2Sum = 0u; + unsigned int g2Sum = 0u; + unsigned int b2Sum = 0u; + unsigned int r3Sum = 0u; + unsigned int g3Sum = 0u; + unsigned int b3Sum = 0u; + unsigned int step = width * bpp; + + // get sum of each color channel + // all cameras should just see blue colors + for (unsigned int i = 0u; i < height; ++i) + { + for (unsigned int j = 0u; j < step; j+=bpp) + { + unsigned int idx = i * step + j; + unsigned int r1 = img1[idx]; + unsigned int g1 = img1[idx+1]; + unsigned int b1 = img1[idx+2]; + r1Sum += r1; + g1Sum += g1; + b1Sum += b1; + + unsigned int r2 = img2[idx]; + unsigned int g2 = img2[idx+1]; + unsigned int b2 = img2[idx+2]; + r2Sum += r2; + g2Sum += g2; + b2Sum += b2; + + unsigned int r3 = img3[idx]; + unsigned int g3 = img3[idx+1]; + unsigned int b3 = img3[idx+2]; + r3Sum += r3; + g3Sum += g3; + b3Sum += b3; + } + } + + // images from camera1 without intrinsics params and + // camera2 with default intrinsic params should be the same + EXPECT_EQ(0u, r1Sum); + EXPECT_EQ(0u, g1Sum); + EXPECT_LT(0u, b1Sum); + EXPECT_EQ(r1Sum, r2Sum); + EXPECT_EQ(g1Sum, g2Sum); + EXPECT_EQ(b1Sum, b2Sum); + + // images from camera2 and camera3 that have different intrinsics params + // should be different. Both cameras should still see the blue box but + // sum of blue pixels should be slightly different + EXPECT_EQ(0u, r3Sum); + EXPECT_EQ(0u, g3Sum); + EXPECT_LT(0u, b3Sum); + EXPECT_EQ(r2Sum, r3Sum); + EXPECT_EQ(g2Sum, g3Sum); + EXPECT_NE(b2Sum, b3Sum); + + delete [] img1; + delete [] img2; + delete [] img3; + + // Clean up rendering ptrs + box.reset(); + blue.reset(); + + // Clean up + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); + mgr.Remove(sensor3->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(CameraSensorTest, CameraIntrinsics) +{ + gz::common::Console::SetVerbosity(2); + CameraIntrinsics(GetParam()); +} + +////////////////////////////////////////////////// +void CameraSensorTest::CameraProjection(const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "camera_projection.sdf"); + sdf::SDFPtr doc(new sdf::SDF()); + sdf::init(doc); + ASSERT_TRUE(sdf::readFile(path, doc)); + ASSERT_NE(nullptr, doc->Root()); + ASSERT_TRUE(doc->Root()->HasElement("model")); + auto modelPtr = doc->Root()->GetElement("model"); + ASSERT_TRUE(modelPtr->HasElement("link")); + auto linkPtr = modelPtr->GetElement("link"); + ASSERT_TRUE(linkPtr->HasElement("sensor")); + + // Camera sensor without projection tag + auto cameraWithoutProjectionsTag = linkPtr->GetElement("sensor"); + + // Camera sensor with projection tag + auto cameraWithProjectionsTag = + linkPtr->GetElement("sensor")->GetNextElement(); + + // Camera sensor with different projection tag + auto cameraWithDiffProjectionsTag = + cameraWithProjectionsTag->GetNextElement(); + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + igndbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + scene->SetAmbientLight(1.0, 1.0, 1.0); + + gz::rendering::VisualPtr root = scene->RootVisual(); + ASSERT_NE(nullptr, root); + + // create blue material + gz::rendering::MaterialPtr blue = scene->CreateMaterial(); + blue->SetAmbient(0.0, 0.0, 0.3); + blue->SetDiffuse(0.0, 0.0, 0.8); + blue->SetSpecular(0.5, 0.5, 0.5); + + // create box visual + gz::rendering::VisualPtr box = scene->CreateVisual("box"); + ASSERT_NE(nullptr, box); + box->AddGeometry(scene->CreateBox()); + box->SetOrigin(0.0, 0.0, 0.0); + box->SetLocalPosition(gz::math::Vector3d(4.0, 1, 0.5)); + box->SetLocalRotation(0, 0, 0); + box->SetMaterial(blue); + scene->DestroyMaterial(blue); + root->AddChild(box); + + // Do the test + gz::sensors::Manager mgr; + + auto *sensor1 = + mgr.CreateSensor(cameraWithoutProjectionsTag); + auto *sensor2 = + mgr.CreateSensor(cameraWithProjectionsTag); + auto *sensor3 = + mgr.CreateSensor(cameraWithDiffProjectionsTag); + + ASSERT_NE(sensor1, nullptr); + ASSERT_NE(sensor2, nullptr); + ASSERT_NE(sensor3, nullptr); + std::string imgTopic1 = "/camera1/image"; + std::string imgTopic2 = "/camera2/image"; + std::string imgTopic3 = "/camera3/image"; + sensor1->SetScene(scene); + sensor2->SetScene(scene); + sensor3->SetScene(scene); + + std::string infoTopic1 = "/camera1/camera_info"; + std::string infoTopic2 = "/camera2/camera_info"; + std::string infoTopic3 = "/camera3/camera_info"; + WaitForMessageTestHelper helper1("/camera1/image"); + WaitForMessageTestHelper helper2(infoTopic1); + WaitForMessageTestHelper helper3("/camera2/image"); + WaitForMessageTestHelper helper4(infoTopic2); + WaitForMessageTestHelper helper5(imgTopic3); + WaitForMessageTestHelper helper6(infoTopic3); + + EXPECT_TRUE(sensor1->HasConnections()); + EXPECT_TRUE(sensor2->HasConnections()); + EXPECT_TRUE(sensor3->HasConnections()); + + // Update once to create image + mgr.RunOnce(std::chrono::steady_clock::duration::zero()); + + EXPECT_TRUE(helper1.WaitForMessage()) << helper1; + EXPECT_TRUE(helper2.WaitForMessage()) << helper2; + EXPECT_TRUE(helper3.WaitForMessage()) << helper3; + EXPECT_TRUE(helper4.WaitForMessage()) << helper4; + EXPECT_TRUE(helper5.WaitForMessage()) << helper5; + EXPECT_TRUE(helper6.WaitForMessage()) << helper6; + + // Subscribe to the camera info topic + gz::msgs::CameraInfo camera1Info, camera2Info, camera3Info; + unsigned int camera1Counter = 0u; + unsigned int camera2Counter = 0u; + unsigned int camera3Counter = 0u; + + std::function camera1InfoCallback = + [&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg) + { + camera1Info = _msg; + camera1Counter++; + }; + std::function camera2InfoCallback = + [&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg) + { + camera2Info = _msg; + camera2Counter++; + }; + std::function camera3InfoCallback = + [&camera3Info, &camera3Counter](const gz::msgs::CameraInfo& _msg) + { + camera3Info = _msg; + camera3Counter++; + }; + + unsigned int height = 1000u; + unsigned int width = 1000u; + unsigned int bpp = 3u; + unsigned int imgBufferSize = width * height * bpp; + unsigned char* img1 = new unsigned char[imgBufferSize]; + unsigned char* img2 = new unsigned char[imgBufferSize]; + unsigned char* img3 = new unsigned char[imgBufferSize]; + unsigned int camera1DataCounter = 0u; + unsigned int camera2DataCounter = 0u; + unsigned int camera3DataCounter = 0u; + std::function camera1Callback = + [&img1, &camera1DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img1, _msg.data().c_str(), imgBufferSize); + camera1DataCounter++; + }; + + std::function camera2Callback = + [&img2, &camera2DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img2, _msg.data().c_str(), imgBufferSize); + camera2DataCounter++; + }; + std::function camera3Callback = + [&img3, &camera3DataCounter, &imgBufferSize](const gz::msgs::Image & _msg) + { + memcpy(img3, _msg.data().c_str(), imgBufferSize); + camera3DataCounter++; + }; + + // Subscribe to the camera topic + gz::transport::Node node; + node.Subscribe(infoTopic1, camera1InfoCallback); + node.Subscribe(infoTopic2, camera2InfoCallback); + node.Subscribe(infoTopic3, camera3InfoCallback); + node.Subscribe(imgTopic1, camera1Callback); + node.Subscribe(imgTopic2, camera2Callback); + node.Subscribe(imgTopic3, camera3Callback); + + // Wait for a few camera frames + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + + // Run to get image and check image format in callback + bool done = false; + int sleep = 0; + int maxSleep = 10; + while (!done && sleep++ < maxSleep) + { + std::lock_guard lock(g_mutex); + done = (camera1Counter > 0u && camera2Counter > 0u && camera3Counter > 0u && + camera1DataCounter > 0u && camera2DataCounter > 0u && + camera3DataCounter > 0u); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + // Image size, focal length and optical center + // Camera sensor without projection tag + // account for error converting gl projection values back to + // cv projection values + double error = 1.0; + EXPECT_EQ(camera1Info.width(), width); + EXPECT_EQ(camera1Info.height(), width); + EXPECT_NEAR(camera1Info.projection().p(0), 866.23, error); + EXPECT_NEAR(camera1Info.projection().p(5), 866.23, error); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(2), 500.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0.0); + EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0.0); + + // Camera sensor with projection tag + EXPECT_EQ(camera2Info.width(), height); + EXPECT_EQ(camera2Info.height(), height); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(0), 866.23); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(5), 866.23); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(2), 500.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 500.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300.0); + EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200.0); + + // Camera sensor with different projection tag + EXPECT_EQ(camera3Info.width(), width); + EXPECT_EQ(camera3Info.height(), height); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(0), 900.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(5), 900.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(2), 501.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(6), 501.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(3), 0.0); + EXPECT_DOUBLE_EQ(camera3Info.projection().p(7), 0.0); + + unsigned int r1Sum = 0u; + unsigned int g1Sum = 0u; + unsigned int b1Sum = 0u; + unsigned int r2Sum = 0u; + unsigned int g2Sum = 0u; + unsigned int b2Sum = 0u; + unsigned int r3Sum = 0u; + unsigned int g3Sum = 0u; + unsigned int b3Sum = 0u; + unsigned int step = width * bpp; + + // get sum of each color channel + // all cameras should just see blue colors + for (unsigned int i = 0u; i < height; ++i) + { + for (unsigned int j = 0u; j < step; j+=bpp) + { + unsigned int idx = i * step + j; + unsigned int r1 = img1[idx]; + unsigned int g1 = img1[idx+1]; + unsigned int b1 = img1[idx+2]; + r1Sum += r1; + g1Sum += g1; + b1Sum += b1; + + unsigned int r2 = img2[idx]; + unsigned int g2 = img2[idx+1]; + unsigned int b2 = img2[idx+2]; + r2Sum += r2; + g2Sum += g2; + b2Sum += b2; + + unsigned int r3 = img3[idx]; + unsigned int g3 = img3[idx+1]; + unsigned int b3 = img3[idx+2]; + r3Sum += r3; + g3Sum += g3; + b3Sum += b3; + } + } + + // images from camera1 without projections params and + // camera2 with default projection params should be the same + EXPECT_EQ(0u, r1Sum); + EXPECT_EQ(0u, g1Sum); + EXPECT_LT(0u, b1Sum); + EXPECT_EQ(r1Sum, r2Sum); + EXPECT_EQ(g1Sum, g2Sum); + EXPECT_EQ(b1Sum, b2Sum); + + // images from camera2 and camera3 that have different projections params + // should be different. Both cameras should still see the blue box but + // sum of blue pixels should be slightly different + EXPECT_EQ(0u, r3Sum); + EXPECT_EQ(0u, g3Sum); + EXPECT_LT(0u, b3Sum); + EXPECT_EQ(r2Sum, r3Sum); + EXPECT_EQ(g2Sum, g3Sum); + EXPECT_NE(b2Sum, b3Sum); + + delete [] img1; + delete [] img2; + delete [] img3; + + // Clean up rendering ptrs + box.reset(); + blue.reset(); + + // Clean up + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); + mgr.Remove(sensor3->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(CameraSensorTest, CameraProjection) +{ + gz::common::Console::SetVerbosity(2); + CameraProjection(GetParam()); +} + ////////////////////////////////////////////////// void CameraSensorTest::ImageFormatLInt8(const std::string &_renderEngine) { diff --git a/test/sdf/camera_intrinsics.sdf b/test/sdf/camera_intrinsics.sdf new file mode 100644 index 00000000..7b5aabea --- /dev/null +++ b/test/sdf/camera_intrinsics.sdf @@ -0,0 +1,77 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + + 10 + base_camera + /camera2/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 866.23 + 866.23 + 500 + 500 + 0 + + + + + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + + + + + + + + diff --git a/test/sdf/camera_projection.sdf b/test/sdf/camera_projection.sdf new file mode 100644 index 00000000..80a0a271 --- /dev/null +++ b/test/sdf/camera_projection.sdf @@ -0,0 +1,70 @@ + + + + + + 10 + base_camera + /camera1/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + + + 10 + base_camera + /camera2/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + + 866.23 + 866.23 + 500 + 500 + 300 + 200 + + + + + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + 0 + + + + + + + From 86c3e1990945e11e200d43f32bdd8143b0ad83a6 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 12 Nov 2021 09:15:08 -0800 Subject: [PATCH 4/9] Destroy rendering sensors when sensor is removed (#169) Signed-off-by: Ian Chen --- src/RenderingSensor.cc | 9 +++++++++ test/integration/camera_plugin.cc | 12 ++++++++++++ 2 files changed, 21 insertions(+) diff --git a/src/RenderingSensor.cc b/src/RenderingSensor.cc index 1c71fd33..4666ca0e 100644 --- a/src/RenderingSensor.cc +++ b/src/RenderingSensor.cc @@ -47,6 +47,15 @@ RenderingSensor::RenderingSensor() : ////////////////////////////////////////////////// RenderingSensor::~RenderingSensor() { + if (!this->dataPtr->scene) + return; + for (auto &s : this->dataPtr->sensors) + { + auto sensor = s.lock(); + if (sensor) + this->dataPtr->scene->DestroySensor(sensor); + } + this->dataPtr->sensors.clear(); } ///////////////////////////////////////////////// diff --git a/test/integration/camera_plugin.cc b/test/integration/camera_plugin.cc index fae2acc9..2a8ab0ff 100644 --- a/test/integration/camera_plugin.cc +++ b/test/integration/camera_plugin.cc @@ -167,6 +167,18 @@ void CameraSensorTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) EXPECT_DOUBLE_EQ(0.0, projection.p(11)); } + // test removing sensor + // first make sure the sensor objects do exist + auto sensorId = sensor->Id(); + auto cameraId = sensor->RenderingCamera()->Id(); + EXPECT_EQ(sensor, mgr.Sensor(sensorId)); + EXPECT_EQ(sensor->RenderingCamera(), scene->SensorById(cameraId)); + // remove and check sensor objects no longer exist in both sensors and + // rendering + EXPECT_TRUE(mgr.Remove(sensorId)); + EXPECT_EQ(nullptr, mgr.Sensor(sensorId)); + EXPECT_EQ(nullptr, scene->SensorById(cameraId)); + // Clean up engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); From 6b8b57025fa40eaf6e1be8e0d4ca005b7c8caef8 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 23 Feb 2023 02:57:47 -0800 Subject: [PATCH 5/9] clean up rendering resources (#324) Signed-off-by: Ian Chen --- src/RenderingSensor.cc | 2 +- test/integration/depth_camera_plugin.cc | 12 +++++------ test/integration/gpu_lidar_sensor_plugin.cc | 24 ++++++++++++++++++--- test/integration/rgbd_camera_plugin.cc | 10 +++++---- test/integration/thermal_camera_plugin.cc | 10 +++++---- 5 files changed, 40 insertions(+), 18 deletions(-) diff --git a/src/RenderingSensor.cc b/src/RenderingSensor.cc index 4666ca0e..a9ce059b 100644 --- a/src/RenderingSensor.cc +++ b/src/RenderingSensor.cc @@ -47,7 +47,7 @@ RenderingSensor::RenderingSensor() : ////////////////////////////////////////////////// RenderingSensor::~RenderingSensor() { - if (!this->dataPtr->scene) + if (!this->dataPtr->scene || !this->dataPtr->scene->IsInitialized()) return; for (auto &s : this->dataPtr->sensors) { diff --git a/test/integration/depth_camera_plugin.cc b/test/integration/depth_camera_plugin.cc index f38fc9dc..b8f50c02 100644 --- a/test/integration/depth_camera_plugin.cc +++ b/test/integration/depth_camera_plugin.cc @@ -222,6 +222,7 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalRotation(0, 0, 0); box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); box->SetMaterial(blue); + scene->DestroyMaterial(blue); root->AddChild(box); // do the test @@ -333,11 +334,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_EQ(9, infoMsg.rectification_matrix().size()); // Check that for a box really close it returns -inf - root->RemoveChild(box); gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; @@ -369,11 +368,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); // Check that for a box really far it returns inf - root->RemoveChild(box); gz::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; @@ -405,11 +402,9 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( // Check that the depth values for a box do not warp. - root->RemoveChild(box); gz::math::Vector3d boxPositionFillFrame( unitBoxSize * 0.5 + 0.2, 0.0, 0.0); box->SetLocalPosition(boxPositionFillFrame); - root->AddChild(box); mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; @@ -494,7 +489,12 @@ void DepthCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); g_pcMutex.unlock(); + // clean up rendering ptrs + blue.reset(); + box.reset(); + // Clean up + mgr.Remove(depthSensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/gpu_lidar_sensor_plugin.cc b/test/integration/gpu_lidar_sensor_plugin.cc index db3a02a9..28e2144e 100644 --- a/test/integration/gpu_lidar_sensor_plugin.cc +++ b/test/integration/gpu_lidar_sensor_plugin.cc @@ -277,6 +277,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Clean up c.reset(); + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -414,8 +415,11 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_TRUE(pointMsgs.back().is_dense()); EXPECT_EQ(32u * horzSamples * vertSamples, pointMsgs.back().data().size()); + // Clean up rendering ptrs + visualBox1.reset(); + // Clean up - // + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -567,7 +571,14 @@ void GpuLidarSensorTest::TestThreeBoxes(const std::string &_renderEngine) for (unsigned int i = 0; i < sensor1->RayCount(); ++i) EXPECT_DOUBLE_EQ(sensor2->Range(i), gz::math::INF_D); + // Clean up rendering ptrs + visualBox1.reset(); + visualBox2.reset(); + visualBox3.reset(); + // Clean up + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -687,7 +698,11 @@ void GpuLidarSensorTest::VerticalLidar(const std::string &_renderEngine) } } + // Clean up rendering ptrs + visualBox1.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -809,8 +824,12 @@ void GpuLidarSensorTest::ManualUpdate(const std::string &_renderEngine) EXPECT_DOUBLE_EQ(sensor2->Range(last), gz::math::INF_D); #endif + // Clean up rendering ptrs + visualBox1.reset(); + // Clean up - // + mgr.Remove(sensor1->Id()); + mgr.Remove(sensor2->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -849,7 +868,6 @@ void GpuLidarSensorTest::Topic(const std::string &_renderEngine) // Create a GpuLidarSensor gz::sensors::Manager mgr; - // Default topic { const std::string topic; diff --git a/test/integration/rgbd_camera_plugin.cc b/test/integration/rgbd_camera_plugin.cc index e0ced44b..4a329976 100644 --- a/test/integration/rgbd_camera_plugin.cc +++ b/test/integration/rgbd_camera_plugin.cc @@ -242,6 +242,7 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( box->SetLocalRotation(0, 0, 0); box->SetLocalScale(unitBoxSize, unitBoxSize, unitBoxSize); box->SetMaterial(blue); + scene->DestroyMaterial(blue); root->AddChild(box); // do the test @@ -526,11 +527,9 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_imgMutex.unlock(); // Check that for a box really close it returns -inf - root->RemoveChild(box); math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(common::Time::Zero, true); for (int sleep = 0; sleep < 300 && @@ -632,11 +631,9 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_pcMutex.unlock(); // Check that for a box really far it returns inf - root->RemoveChild(box); math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(common::Time::Zero, true); for (int sleep = 0; sleep < 300 && @@ -737,7 +734,12 @@ void RgbdCameraSensorTest::ImagesWithBuiltinSDF( g_imgMutex.unlock(); g_pcMutex.unlock(); + // Clean up rendering ptrs + box.reset(); + blue.reset(); + // Clean up + mgr.Remove(rgbdSensor->Id()); engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/thermal_camera_plugin.cc b/test/integration/thermal_camera_plugin.cc index 5b1f71c4..bb3e9118 100644 --- a/test/integration/thermal_camera_plugin.cc +++ b/test/integration/thermal_camera_plugin.cc @@ -262,11 +262,9 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( EXPECT_EQ(9, infoMsg.rectification_matrix().size()); // Check that for a box really close it returns box temperature - root->RemoveChild(box); gz::math::Vector3d boxPositionNear( unitBoxSize * 0.5 + near_ * 0.5, 0.0, 0.0); box->SetLocalPosition(boxPositionNear); - root->AddChild(box); mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; @@ -296,11 +294,9 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_mutex.unlock(); // Check that for a box really far it returns ambient temperature - root->RemoveChild(box); gz::math::Vector3d boxPositionFar( unitBoxSize * 0.5 + far_ * 1.5, 0.0, 0.0); box->SetLocalPosition(boxPositionFar); - root->AddChild(box); mgr.RunOnce(gz::common::Time::Zero, true); for (int sleep = 0; @@ -329,8 +325,14 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( g_infoMutex.unlock(); g_mutex.unlock(); + delete [] g_thermalBuffer; + g_thermalBuffer = nullptr; + + // Clean up rendering ptrs + box.reset(); // Clean up + mgr.Remove(thermalSensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } From 67b5b6eaaa131460308837c19f43011a80a6797d Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 13 Nov 2023 09:47:15 -0600 Subject: [PATCH 6/9] Update github action workflows (#401) * Use on `push` only on stable branches to avoid duplicate runs * Update project automation Signed-off-by: Addisu Z. Taddese --- .github/workflows/ci.yml | 12 +++++++++--- .github/workflows/triage.yml | 9 +++------ 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 744bfcff..a95c19bb 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,12 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'ign-sensors[0-9]' + - 'gz-sensors[0-9]?' + - 'main' jobs: bionic-ci: @@ -8,7 +14,7 @@ jobs: name: Ubuntu Bionic CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@bionic @@ -19,7 +25,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Compile and test id: ci uses: ignition-tooling/action-ignition-ci@focal diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 736670e0..2332244b 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -10,10 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Add ticket to inbox - uses: technote-space/create-project-card-action@v1 + uses: actions/add-to-project@v0.5.0 with: - PROJECT: Core development - COLUMN: Inbox - GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} - CHECK_ORG_PROJECT: true - + project-url: https://github.com/orgs/gazebosim/projects/7 + github-token: ${{ secrets.TRIAGE_TOKEN }} From b7148d659ea430e93b9b1b14f3b8162386636a58 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 5 Jan 2024 15:44:02 -0800 Subject: [PATCH 7/9] Prepare for 3.6.0 release (#407) Signed-off-by: Ian Chen --- CMakeLists.txt | 2 +- Changelog.md | 25 +++++++++++++++++++++++++ 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e9ccbffd..b37d3994 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-sensors3 VERSION 3.5.0) +project(ignition-sensors3 VERSION 3.6.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 3ca00260..d04d9bb2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,30 @@ ## Gazebo Sensors 3 +### Gazebo Sensors 3.6.0 (2024-01-05) + +1. Update github action workflows + * [Pull request #401](https://github.com/gazebosim/gz-sensors/pull/401) + * [Pull request #335](https://github.com/gazebosim/gz-sensors/pull/335) + * [Pull request #334](https://github.com/gazebosim/gz-sensors/pull/334) + +1. Clean up rendering resources + * [Pull request #324](https://github.com/gazebosim/gz-sensors/pull/324) + +1. Destroy rendering sensors when sensor is removed + * [Pull request #169](https://github.com/gazebosim/gz-sensors/pull/169) + +1. Support protobuf >= 22 + * [Pull request #351](https://github.com/gazebosim/gz-sensors/pull/351) + +1. Fix links in Changelog + * [Pull request #330](https://github.com/gazebosim/gz-sensors/pull/330) + +1. Fix Camera info test + * [Pull request #326](https://github.com/gazebosim/gz-sensors/pull/326) + +1. Added Camera Info topic support for cameras + * [Pull request #285](https://github.com/gazebosim/gz-sensors/pull/285) + ### Gazebo Sensors 3.5.0 (2022-11-30) 1. Add missing DEPENDS_ON_COMPONENTS parameters. From ff5bbb1d74859f7fd3a3382749ef5ef8bd67fa6e Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 12 Jan 2024 11:10:37 -0600 Subject: [PATCH 8/9] Allow specifying gz_frame_id as an alternative to ignition_frame_id (#409) This is to help migration to newer versions of Gazebo and allows users to replace ignition_frame_id in their SDF files while still in Fortress. Signed-off-by: Addisu Z. Taddese --- src/Sensor.cc | 12 ++++++++++ src/Sensor_TEST.cc | 55 ++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 67 insertions(+) diff --git a/src/Sensor.cc b/src/Sensor.cc index ad884105..bc25f7ec 100644 --- a/src/Sensor.cc +++ b/src/Sensor.cc @@ -148,6 +148,18 @@ bool SensorPrivate::PopulateFromSDF(const sdf::Sensor &_sdf) if (element->HasElement("ignition_frame_id")) { this->frame_id = element->Get("ignition_frame_id"); + // Warn if both ignition_frame_id and gz_frame_id are specified + if (element->HasElement("gz_frame_id")) + { + ignwarn << "Found both `ignition_frame_id` and `gz_frame_id` in sensor" + << this->name << ". Only `ignition_frame_id` will be used\n"; + } + } + else if (element->HasElement("gz_frame_id")) + { + // Also read gz_frame_id to support SDF that's compatible with newer + // versions of Gazebo. + this->frame_id = element->Get("gz_frame_id"); } else { diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc index 79eb9cde..9bc4d5b6 100644 --- a/src/Sensor_TEST.cc +++ b/src/Sensor_TEST.cc @@ -350,6 +350,61 @@ TEST(Sensor_TEST, SetRateZeroService) EXPECT_FLOAT_EQ(20.0, sensor.UpdateRate()); } +////////////////////////////////////////////////// +TEST(Sensor_TEST, FrameIdFromSdf) +{ + auto loadSensorWithSdfParam = + [](TestSensor &_testSensor, const std::string &_sensorParam) + { + const std::string sensorSdf = R"( + + + + )" + + _sensorParam + R"( + + + + + )"; + sdf::Root root; + sdf::Errors errors = root.LoadSdfString(sensorSdf); + ASSERT_TRUE(errors.empty()) << errors; + auto *model = root.Model(); + ASSERT_NE(model, nullptr); + auto *link = model->LinkByIndex(0); + ASSERT_NE(link, nullptr); + auto *sensor = link->SensorByIndex(0); + ASSERT_NE(sensor, nullptr); + + _testSensor.Load(*sensor); + }; + + { + TestSensor testSensor; + loadSensorWithSdfParam(testSensor, ""); + EXPECT_EQ("test", testSensor.FrameId()); + } + { + TestSensor testSensor; + loadSensorWithSdfParam( + testSensor, "custom_frame_id"); + EXPECT_EQ("custom_frame_id", testSensor.FrameId()); + } + { + TestSensor testSensor; + loadSensorWithSdfParam(testSensor, + "custom_frame_id"); + EXPECT_EQ("custom_frame_id", testSensor.FrameId()); + } + { + TestSensor testSensor; + loadSensorWithSdfParam(testSensor, R"( + custom_frame_id + other_custom_frame_id)"); + EXPECT_EQ("custom_frame_id", testSensor.FrameId()); + } +} ////////////////////////////////////////////////// int main(int argc, char **argv) From 6d1f163cb1a3bb7389dc0a1233104566e69204b9 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Fri, 12 Jan 2024 17:33:28 -0600 Subject: [PATCH 9/9] Prepare for 6.8.0 (#411) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 11 +++++++++++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index fdeab814..f1832f12 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-sensors6 VERSION 6.7.1) +project(ignition-sensors6 VERSION 6.8.0) #============================================================================ # Find ignition-cmake diff --git a/Changelog.md b/Changelog.md index 27c2b883..777e5649 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,16 @@ ## Gazebo Sensors 6 +### Gazebo Sensors 6.8.0 (2024-01-12) + +1. Allow specifying gz_frame_id as an alternative to ignition_frame_id + * [Pull request #409](https://github.com/gazebosim/gz-sensors/pull/409) + +1. [backport fortress] camera info topic published with the right data + * [Pull request #383](https://github.com/gazebosim/gz-sensors/pull/383) + +1. Infrastructure + * [Pull request #401](https://github.com/gazebosim/gz-sensors/pull/401) + ### Gazebo Sensors 6.7.1 (2023-09-01) 1. Support protobuf >= 22