diff --git a/.github/disabled_workflows/macos.yml b/.github/disabled_workflows/macos.yml index a2461277..2b2353f2 100644 --- a/.github/disabled_workflows/macos.yml +++ b/.github/disabled_workflows/macos.yml @@ -9,7 +9,7 @@ jobs: PACKAGE: gz-sensors8 runs-on: macos-latest steps: - - uses: actions/checkout@v2 + - uses: actions/checkout@v3 - name: Set up Homebrew id: set-up-homebrew uses: Homebrew/actions/setup-homebrew@master diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index bd14ed68..ca539218 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -8,7 +8,7 @@ jobs: name: Ubuntu Focal CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@focal @@ -21,7 +21,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - name: Compile and test id: ci uses: gazebo-tooling/action-gz-ci@jammy diff --git a/CMakeLists.txt b/CMakeLists.txt index 0a8792cb..7d9fb8a3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -89,7 +89,7 @@ set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) #-------------------------------------- # Find SDFormat -gz_find_package(sdformat13 REQUIRED) +gz_find_package(sdformat13 VERSION 13.3 REQUIRED) set(SDF_VER ${sdformat13_VERSION_MAJOR}) #-------------------------------------- diff --git a/COPYING b/COPYING deleted file mode 100644 index 4909afd0..00000000 --- a/COPYING +++ /dev/null @@ -1,178 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - diff --git a/Changelog.md b/Changelog.md index cc8041db..b0b4ac00 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,6 +4,38 @@ ## Gazebo Sensors 7 +### Gazebo Sensors 7.2.0 (2023-04-13) + +1. Cleanup resources in CameraSensor destructor + * [Pull request #340](https://github.com/gazebosim/gz-sensors/pull/340) + +1. CI workflow: use checkout v3 + * [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 flaky triggered bounding box camera test + * [Pull request #329](https://github.com/gazebosim/gz-sensors/pull/329) + +1. Fix Camera info test + * [Pull request #326](https://github.com/gazebosim/gz-sensors/pull/326) + +1. Added trigger to BoundingBoxCamera + * [Pull request #322](https://github.com/gazebosim/gz-sensors/pull/322) + +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) + +1. ign -> gz Migrate Ignition Headers : gz-sensors + * [Pull request #260](https://github.com/gazebosim/gz-sensors/pull/260) + ### Gazebo Sensors 7.1.0 (2023-02-09) 1. Added airspeed sensor @@ -143,6 +175,29 @@ ## Gazebo Sensors 6 +### Gazebo Sensors 6.7.0 (2023-02-13) + +1. Disable thermal camera test on MacOS. + * [Pull request #243](https://github.com/gazebosim/gz-sensors/pull/243) + +1. Add optional optical frame id to camera sensors. + * [Pull request #259](https://github.com/gazebosim/gz-sensors/pull/259) + +1. Add support for 16 bit image format. + * [Pull request #276](https://github.com/gazebosim/gz-sensors/pull/276) + +1. Fix navsat frame id. + * [Pull request #298](https://github.com/gazebosim/gz-sensors/pull/298) + +1. CameraInfo is now published when there's a CameraSensor subscriber. + * [Pull request #308](https://github.com/gazebosim/gz-sensors/pull/308) + +1. Add HasInfoConnections() method to expose infoPub.HasConnections() to + child of CameraSensor class. + * [Pull request #310](https://github.com/gazebosim/gz-sensors/pull/310) + +1. Forward port 3.5.0. + ### Gazebo Sensors 6.6.0 (2022-06-17) 1. Add BoundingBox Sensor @@ -231,7 +286,7 @@ 1. Trivial tutorial typo correction in Custom Sensors tutorial * [Pull request #160](https://github.com/gazebosim/gz-sensors/pull/160) -1. Bumps in fortress: gz-sensors6 +1. Bumps in fortress: ign-sensors6 * [Pull request #120](https://github.com/gazebosim/gz-sensors/pull/120) 1. Port codecov to new configuration @@ -261,7 +316,7 @@ ### Gazebo Sensors 5.1.0 (2021-10-15) -1. Depend on gz-msgs 7.2 and libSDFormat 11.3 +1. Depend on ign-msgs 7.2 and libSDFormat 11.3 * [Pull request #154](https://github.com/gazebosim/gz-sensors/pull/154) 1. 👩‍🌾 Print debug messages when sensors advertise topics @@ -290,16 +345,16 @@ ### Gazebo Sensors 5.0.0 (2021-03-30) -1. Bump in edifice: gz-common4 +1. Bump in edifice: ign-common4 * [Pull request #85](https://github.com/gazebosim/gz-sensors/pull/85) 1. Bump in edifice: sdformat11 * [Pull request #78](https://github.com/gazebosim/gz-sensors/pull/78) -1. Bump in edifice: gz-msgs7 +1. Bump in edifice: ign-msgs7 * [Pull request #75](https://github.com/gazebosim/gz-sensors/pull/75) -1. Bump in edifice: gz-rendering5 +1. Bump in edifice: ign-rendering5 * [Pull request #55](https://github.com/gazebosim/gz-sensors/pull/55) 1. Documentation updates @@ -365,13 +420,13 @@ 1. Move installation instructions from README.md to Installation tutorial * [Pull request 50](https://github.com/gazebosim/gz-sensors/pull/50) -1. Bump gz-math to 6.6 +1. Bump ign-math to 6.6 * [Pull request 48](https://github.com/gazebosim/gz-sensors/pull/48) 1. Replaced common::Time with std::chrono * [Pull request 41](https://github.com/gazebosim/gz-sensors/pull/41) -1. Depend on gz-msgs6, gz-transport9, sdf10 +1. Depend on ign-msgs6, ign-transport9, sdf10 * [Pull request 31](https://github.com/gazebosim/gz-sensors/pull/31) 1. GitHub migration @@ -382,7 +437,7 @@ 1. Set camera sensor visibility mask * [BitBucket pull request 115](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/115) -1. Depend on gz-rendering4 +1. Depend on ign-rendering4 * [BitBucket pull request 111](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/111) ## Gazebo Sensors 3 @@ -491,13 +546,13 @@ 1. Add support for sdformat frame semantics * [BitBucket pull request 104](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/104) -1. Remove deprecations in gz-sensors3 +1. Remove deprecations in ign-sensors3 * [BitBucket pull request 103](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/103) 1. Break out image noise classes * [BitBucket pull request 102](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/102) -1. Depend on gz-transport8, gz-msgs5, sdformat9 +1. Depend on ign-transport8, ign-msgs5, sdformat9 * [BitBucket pull request 101](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/101) * [BitBucket pull request 105](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/105) @@ -510,7 +565,7 @@ 1. Removed deprecations from Manager. * [BitBucket pull request 99](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/99) -1. Depend on gz-rendering3 +1. Depend on ign-rendering3 * [BitBucket pull request 88](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/88) ## Gazebo Sensors 2 @@ -551,7 +606,7 @@ ### Gazebo Sensors 2.6.0 (2019-08-27) -1. Update depth and rgbd camera sensor to output point cloud data generated by gz-rendering DepthCamera +1. Update depth and rgbd camera sensor to output point cloud data generated by ign-rendering DepthCamera * [BitBucket pull request 91](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/91) ### Gazebo Sensors 2.5.1 (2019-08-12) @@ -561,7 +616,7 @@ ### Gazebo Sensors 2.5.0 -1. Add `GZ_PROFILER_ENABLE` cmake option for enabling the gz-common profiler. +1. Add `IGN_PROFILER_ENABLE` cmake option for enabling the ign-common profiler. * [BitBucket pull request 82](https://osrf-migration.github.io/ignition-gh-pages/#!/ignitionrobotics/ign-sensors/pull-requests/82) 1. Deduplicate `frame_ids` from sensor message headers diff --git a/LICENSE b/LICENSE index e3d1f5e4..4909afd0 100644 --- a/LICENSE +++ b/LICENSE @@ -1,15 +1,178 @@ -Software License Agreement (Apache License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright 2017 Open Source Robotics Foundation + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION -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 + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS - 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. diff --git a/include/gz/sensors/BoundingBoxCameraSensor.hh b/include/gz/sensors/BoundingBoxCameraSensor.hh index 80fb6686..4fe47496 100644 --- a/include/gz/sensors/BoundingBoxCameraSensor.hh +++ b/include/gz/sensors/BoundingBoxCameraSensor.hh @@ -103,6 +103,10 @@ namespace gz /// \return True on success. private: bool CreateCamera(); + /// \brief Callback for triggered subscription + /// \param[in] _msg Boolean message + private: void OnTrigger(const gz::msgs::Boolean &/*_msg*/); + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/include/gz/sensors/CameraSensor.hh b/include/gz/sensors/CameraSensor.hh index eaf9e3ba..a9cefa78 100644 --- a/include/gz/sensors/CameraSensor.hh +++ b/include/gz/sensors/CameraSensor.hh @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -173,6 +174,10 @@ namespace gz /// \param[in] _scene Pointer to the new scene. private: void OnSceneChange(gz::rendering::ScenePtr /*_scene*/); + /// \brief Callback for triggered subscription + /// \param[in] _msg Boolean message + private: void OnTrigger(const gz::msgs::Boolean &/*_msg*/); + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING /// \brief Data pointer for private data /// \internal diff --git a/src/BoundingBoxCameraSensor.cc b/src/BoundingBoxCameraSensor.cc index 5ea79b8c..777dc14a 100644 --- a/src/BoundingBoxCameraSensor.cc +++ b/src/BoundingBoxCameraSensor.cc @@ -85,6 +85,15 @@ class gz::sensors::BoundingBoxCameraSensorPrivate /// \brief Just a mutex for thread safety public: std::mutex mutex; + /// \brief True if camera is triggered by a topic + public: bool isTriggeredCamera{false}; + + /// \brief True if camera has been triggered by a topic + public: bool isTriggered{false}; + + /// \brief Topic for camera trigger + public: std::string triggerTopic{""}; + /// \brief BoundingBoxes type public: rendering::BoundingBoxType type {rendering::BoundingBoxType::BBT_VISIBLEBOX2D}; @@ -217,6 +226,34 @@ bool BoundingBoxCameraSensor::Load(const sdf::Sensor &_sdf) gzdbg << "Bounding boxes for [" << this->Name() << "] advertised on [" << topicBoundingBoxes << std::endl; + if (_sdf.CameraSensor()->Triggered()) + { + if (!_sdf.CameraSensor()->TriggerTopic().empty()) + { + this->dataPtr->triggerTopic = _sdf.CameraSensor()->TriggerTopic(); + } + else + { + this->dataPtr->triggerTopic = + transport::TopicUtils::AsValidTopic( + this->Topic() + "/trigger"); + + if (this->dataPtr->triggerTopic.empty()) + { + gzerr << "Invalid trigger topic name [" << + this->dataPtr->triggerTopic << "]" << std::endl; + return false; + } + } + + this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic, + &BoundingBoxCameraSensor::OnTrigger, this); + + gzdbg << "Camera trigger messages for [" << this->Name() << "] subscribed" + << " on [" << this->dataPtr->triggerTopic << "]" << std::endl; + this->dataPtr->isTriggeredCamera = true; + } + if (!this->AdvertiseInfo()) return false; @@ -390,6 +427,16 @@ bool BoundingBoxCameraSensor::Update( this->PublishInfo(_now); } + // render only if necessary + { + std::lock_guard lock(this->dataPtr->mutex); + if (this->dataPtr->isTriggeredCamera && + !this->dataPtr->isTriggered) + { + return true; + } + } + // don't render if there are no subscribers nor saving if (!this->dataPtr->imagePublisher.HasConnections() && !this->dataPtr->boxesPublisher.HasConnections() && @@ -528,6 +575,11 @@ bool BoundingBoxCameraSensor::Update( ++this->dataPtr->saveCounter; } + if (this->dataPtr->isTriggeredCamera) + { + return this->dataPtr->isTriggered = false; + } + return true; } @@ -543,6 +595,13 @@ unsigned int BoundingBoxCameraSensor::ImageWidth() const return this->dataPtr->rgbCamera->ImageWidth(); } +////////////////////////////////////////////////// +void BoundingBoxCameraSensor::OnTrigger(const gz::msgs::Boolean &/*_msg*/) +{ + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->isTriggered = true; +} + ////////////////////////////////////////////////// void BoundingBoxCameraSensorPrivate::SaveImage() { diff --git a/src/CameraSensor.cc b/src/CameraSensor.cc index f908dbc7..977c7bdc 100644 --- a/src/CameraSensor.cc +++ b/src/CameraSensor.cc @@ -49,10 +49,6 @@ using namespace sensors; /// \brief Private data for CameraSensor class gz::sensors::CameraSensorPrivate { - /// \brief Callback for triggered subscription - /// \param[in] _msg Boolean message - public: void OnTrigger(const msgs::Boolean &_msg); - /// \brief Save an image /// \param[in] _data the image data to be saved /// \param[in] _width width of image in pixels @@ -282,6 +278,18 @@ bool CameraSensor::CreateCamera() case sdf::PixelFormatType::L_INT16: this->dataPtr->camera->SetImageFormat(rendering::PF_L16); break; + case sdf::PixelFormatType::BAYER_RGGB8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_RGGB8); + break; + case sdf::PixelFormatType::BAYER_BGGR8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_BGGR8); + break; + case sdf::PixelFormatType::BAYER_GBRG8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_GBRG8); + break; + case sdf::PixelFormatType::BAYER_GRBG8: + this->dataPtr->camera->SetImageFormat(rendering::PF_BAYER_GRBG8); + break; default: gzerr << "Unsupported pixel format [" << static_cast(pixelFormat) << "]\n"; @@ -356,6 +364,24 @@ bool CameraSensor::CreateCamera() 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); @@ -372,6 +398,10 @@ CameraSensor::CameraSensor() ////////////////////////////////////////////////// CameraSensor::~CameraSensor() { + if (this->Scene() && this->dataPtr->camera) + { + this->Scene()->DestroySensor(this->dataPtr->camera); + } } ////////////////////////////////////////////////// @@ -409,6 +439,11 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) if (this->Topic().empty()) this->SetTopic("/camera"); + if (!_sdf.CameraSensor()->CameraInfoTopic().empty()) + { + this->dataPtr->infoTopic = _sdf.CameraSensor()->CameraInfoTopic(); + } + this->dataPtr->pub = this->dataPtr->node.Advertise( this->Topic()); @@ -431,7 +466,8 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) else { this->dataPtr->triggerTopic = - transport::TopicUtils::AsValidTopic(this->dataPtr->triggerTopic); + transport::TopicUtils::AsValidTopic( + this->Topic() + "/trigger"); if (this->dataPtr->triggerTopic.empty()) { @@ -441,7 +477,7 @@ bool CameraSensor::Load(const sdf::Sensor &_sdf) } this->dataPtr->node.Subscribe(this->dataPtr->triggerTopic, - &CameraSensorPrivate::OnTrigger, this->dataPtr.get()); + &CameraSensor::OnTrigger, this); gzdbg << "Camera trigger messages for [" << this->Name() << "] subscribed" << " on [" << this->dataPtr->triggerTopic << "]" << std::endl; @@ -581,6 +617,22 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) format = common::Image::L_INT16; msgsPixelFormat = msgs::PixelFormatType::L_INT16; break; + case rendering::PF_BAYER_RGGB8: + format = common::Image::BAYER_RGGB8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_RGGB8; + break; + case rendering::PF_BAYER_BGGR8: + format = common::Image::BAYER_BGGR8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_BGGR8; + break; + case rendering::PF_BAYER_GBRG8: + format = common::Image::BAYER_GBRG8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_GBRG8; + break; + case rendering::PF_BAYER_GRBG8: + format = common::Image::BAYER_GRBG8; + msgsPixelFormat = msgs::PixelFormatType::BAYER_GRBG8; + break; default: gzerr << "Unsupported pixel format [" << this->dataPtr->camera->ImageFormat() << "]\n"; @@ -639,10 +691,10 @@ bool CameraSensor::Update(const std::chrono::steady_clock::duration &_now) } ////////////////////////////////////////////////// -void CameraSensorPrivate::OnTrigger(const gz::msgs::Boolean &/*_msg*/) +void CameraSensor::OnTrigger(const gz::msgs::Boolean &/*_msg*/) { - std::lock_guard lock(this->mutex); - this->isTriggered = true; + std::lock_guard lock(this->dataPtr->mutex); + this->dataPtr->isTriggered = true; } ////////////////////////////////////////////////// @@ -700,17 +752,17 @@ std::string CameraSensor::InfoTopic() const ////////////////////////////////////////////////// bool CameraSensor::AdvertiseInfo() { - // TODO(anyone) Make info topic configurable from SDF - // Info topic must be at same level as image topic - auto parts = common::Split(this->Topic(), '/'); - parts.pop_back(); - - for (const auto &part : parts) + if (this->dataPtr->infoTopic.empty()) { - if (!part.empty()) - this->dataPtr->infoTopic += "/" + part; + auto parts = common::Split(this->Topic(), '/'); + parts.pop_back(); + for (const auto &part : parts) + { + if (!part.empty()) + this->dataPtr->infoTopic += "/" + part; + } + this->dataPtr->infoTopic += "/camera_info"; } - this->dataPtr->infoTopic += "/camera_info"; return this->AdvertiseInfo(this->dataPtr->infoTopic); } diff --git a/src/Camera_TEST.cc b/src/Camera_TEST.cc index 89cc5751..6c775ec4 100644 --- a/src/Camera_TEST.cc +++ b/src/Camera_TEST.cc @@ -48,7 +48,8 @@ sdf::ElementPtr cameraToBadSdf() sdf::ElementPtr CameraToSdf(const std::string &_type, const std::string &_name, double _updateRate, - const std::string &_topic, bool _alwaysOn, bool _visualize) + const std::string &_topic, const std::string &_cameraInfoTopic, + bool _alwaysOn, bool _visualize) { std::ostringstream stream; stream @@ -62,6 +63,8 @@ sdf::ElementPtr CameraToSdf(const std::string &_type, << " "<< _alwaysOn <<"" << " " << _visualize << "" << " " + << " " << _cameraInfoTopic + << "" << " .75" << " " << " 640" @@ -146,8 +149,8 @@ TEST(Camera_TEST, CreateCamera) { gz::sensors::Manager mgr; - sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, "/cam", - true, true); + sdf::ElementPtr camSdf = CameraToSdf("camera", "my_camera", 60.0, + "/cam", "my_camera/camera_info", true, true); // Create a CameraSensor gz::sensors::CameraSensor *cam = @@ -158,7 +161,7 @@ TEST(Camera_TEST, CreateCamera) // Check topics EXPECT_EQ("/cam", cam->Topic()); - EXPECT_EQ("/camera_info", cam->InfoTopic()); + EXPECT_EQ("my_camera/camera_info", cam->InfoTopic()); // however camera is not loaded because a rendering scene is missing so // updates will not be successful and image size will be 0 @@ -190,19 +193,21 @@ TEST(Camera_TEST, Topic) // Default topic { const std::string topic; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, - visualize); + const std::string cameraInfoTopic; + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, cameraInfoTopic, + alwaysOn, visualize); auto camera = mgr.CreateSensor(cameraSdf); ASSERT_NE(nullptr, camera); EXPECT_NE(gz::sensors::NO_SENSOR, camera->Id()); EXPECT_EQ("/camera", camera->Topic()); + EXPECT_EQ("/camera_info", camera->InfoTopic()); } // Convert to valid topic { const std::string topic = "/topic with spaces/@~characters//"; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, "", alwaysOn, visualize); auto camera = mgr.CreateSensor(cameraSdf); @@ -210,12 +215,13 @@ TEST(Camera_TEST, Topic) EXPECT_NE(gz::sensors::NO_SENSOR, camera->Id()); EXPECT_EQ("/topic_with_spaces/characters", camera->Topic()); + EXPECT_EQ("/topic_with_spaces/camera_info", camera->InfoTopic()); } // Invalid topic { const std::string topic = "@@@"; - auto cameraSdf = CameraToSdf(type, name, updateRate, topic, alwaysOn, + auto cameraSdf = CameraToSdf(type, name, updateRate, topic, "", alwaysOn, visualize); auto sensor = mgr.CreateSensor(cameraSdf); diff --git a/src/RenderingSensor.cc b/src/RenderingSensor.cc index 29e1fa47..0678e399 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/CMakeLists.txt b/test/integration/CMakeLists.txt index 047a94c2..0b9de3fa 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -12,6 +12,7 @@ set(dri_tests thermal_camera.cc triggered_camera.cc wide_angle_camera.cc + triggered_boundingbox_camera.cc ) set(tests diff --git a/test/integration/boundingbox_camera.cc b/test/integration/boundingbox_camera.cc index dbf8d391..715e2ce6 100644 --- a/test/integration/boundingbox_camera.cc +++ b/test/integration/boundingbox_camera.cc @@ -344,7 +344,11 @@ void BoundingBoxCameraSensorTest::BoxesWithBuiltinSDF( g_mutex.unlock(); + // Clean up rendering ptrs + camera.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); } @@ -465,7 +469,11 @@ void BoundingBoxCameraSensorTest::Boxes3DWithBuiltinSDF( g_mutex.unlock(); + // Clean up rendering ptrs + camera.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/camera.cc b/test/integration/camera.cc index db3d33a6..23eb20a1 100644 --- a/test/integration/camera.cc +++ b/test/integration/camera.cc @@ -240,6 +240,7 @@ void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine) 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 @@ -462,7 +463,14 @@ void CameraSensorTest::CameraIntrinsics(const std::string &_renderEngine) 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()); } @@ -489,13 +497,17 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) auto linkPtr = modelPtr->GetElement("link"); ASSERT_TRUE(linkPtr->HasElement("sensor")); - // Camera sensor without intrinsics tag - auto cameraWithoutIntrinsicsTag = linkPtr->GetElement("sensor"); + // Camera sensor without projection tag + auto cameraWithoutProjectionsTag = linkPtr->GetElement("sensor"); - // Camera sensor with intrinsics tag - auto cameraWithIntrinsicsTag = + // 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) @@ -506,27 +518,61 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) } 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(cameraWithoutIntrinsicsTag); + mgr.CreateSensor(cameraWithoutProjectionsTag); auto *sensor2 = - mgr.CreateSensor(cameraWithIntrinsicsTag); + 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()); @@ -535,28 +581,72 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) 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; - unsigned int camera1Counter = 0; - unsigned int camera2Counter = 0; + 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++; + [&camera1Info, &camera1Counter](const gz::msgs::CameraInfo& _msg) + { + camera1Info = _msg; + camera1Counter++; }; - std::function camera2InfoCallback = - [&camera2Info, &camera2Counter](const gz::msgs::CameraInfo& _msg) { - camera2Info = _msg; - camera2Counter++; + [&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); @@ -568,33 +658,118 @@ void CameraSensorTest::CameraProjection(const std::string &_renderEngine) while (!done && sleep++ < maxSleep) { std::lock_guard lock(g_mutex); - done = (camera1Counter > 0 && camera2Counter > 0); + 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 - double error = 1e-1; - EXPECT_EQ(camera1Info.width(), 1000u); - EXPECT_EQ(camera1Info.height(), 1000u); - EXPECT_NEAR(camera1Info.projection().p(0), 863.22975158691406, error); - EXPECT_NEAR(camera1Info.projection().p(5), 863.22975158691406, error); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(2), 500); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(6), 500); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(3), 0); - EXPECT_DOUBLE_EQ(camera1Info.projection().p(7), 0); + // 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(), 1000u); - EXPECT_EQ(camera2Info.height(), 1000u); + 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), 966.23); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(2), 500); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(6), 400); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(3), 300); - EXPECT_DOUBLE_EQ(camera2Info.projection().p(7), 200); + 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()); } diff --git a/test/integration/depth_camera.cc b/test/integration/depth_camera.cc index 43b726aa..e47f40ad 100644 --- a/test/integration/depth_camera.cc +++ b/test/integration/depth_camera.cc @@ -227,6 +227,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 @@ -341,11 +342,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -377,11 +376,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -413,11 +410,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -502,7 +497,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.cc b/test/integration/gpu_lidar_sensor.cc index c713f2fb..a0753b40 100644 --- a/test/integration/gpu_lidar_sensor.cc +++ b/test/integration/gpu_lidar_sensor.cc @@ -286,6 +286,7 @@ void GpuLidarSensorTest::CreateGpuLidar(const std::string &_renderEngine) // Clean up c.reset(); + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } @@ -424,8 +425,11 @@ void GpuLidarSensorTest::DetectBox(const std::string &_renderEngine) EXPECT_FALSE(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()); } @@ -581,7 +585,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()); } @@ -701,7 +712,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()); } @@ -827,8 +842,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()); } @@ -867,7 +886,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.cc b/test/integration/rgbd_camera.cc index fc778eb5..bc1fd184 100644 --- a/test/integration/rgbd_camera.cc +++ b/test/integration/rgbd_camera.cc @@ -248,6 +248,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 @@ -535,11 +536,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && @@ -641,11 +640,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; sleep < 300 && @@ -746,7 +743,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/segmentation_camera.cc b/test/integration/segmentation_camera.cc index 3015ea6e..20788794 100644 --- a/test/integration/segmentation_camera.cc +++ b/test/integration/segmentation_camera.cc @@ -344,7 +344,11 @@ void SegmentationCameraSensorTest::ImagesWithBuiltinSDF( } } + // Clean up rendering ptrs + camera.reset(); + // Clean up + mgr.Remove(sensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/thermal_camera.cc b/test/integration/thermal_camera.cc index 94db0b14..ff731620 100644 --- a/test/integration/thermal_camera.cc +++ b/test/integration/thermal_camera.cc @@ -288,11 +288,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -322,11 +320,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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -358,7 +354,11 @@ void ThermalCameraSensorTest::ImagesWithBuiltinSDF( 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()); } @@ -549,11 +549,9 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( 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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -584,11 +582,9 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( 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(std::chrono::steady_clock::duration::zero(), true); for (int sleep = 0; @@ -620,7 +616,11 @@ void ThermalCameraSensorTest::Images8BitWithBuiltinSDF( delete [] g_thermalBuffer8Bit; g_thermalBuffer8Bit = nullptr; + // Clean up rendering ptrs + box.reset(); + // Clean up + mgr.Remove(thermalSensor->Id()); engine->DestroyScene(scene); gz::rendering::unloadEngine(engine->Name()); } diff --git a/test/integration/triggered_boundingbox_camera.cc b/test/integration/triggered_boundingbox_camera.cc new file mode 100644 index 00000000..2cc37558 --- /dev/null +++ b/test/integration/triggered_boundingbox_camera.cc @@ -0,0 +1,322 @@ +/* + * Copyright (C) 2023 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 +#include + +#include +#include + +#include +#include +#include +#include + +// TODO(louise) Remove these pragmas once gz-rendering is disabling the +// warnings +#ifdef _WIN32 +#pragma warning(push) +#pragma warning(disable: 4251) +#endif +#include +#include +#include +#ifdef _WIN32 +#pragma warning(pop) +#endif + +#include "test_config.hh" // NOLINT(build/include) +#include "TransportTestTools.hh" + +using namespace std::chrono_literals; + +class TriggeredBoundingBoxCameraTest: public testing::Test, + public testing::WithParamInterface +{ + // Documentation inherited + protected: void SetUp() override + { + // Disable Ogre tests on windows. See + // https://github.com/gazebosim/gz-sensors/issues/284 +#ifdef _WIN32 + if (strcmp(GetParam(), "ogre") == 0) + { + GTEST_SKIP() << "Ogre tests disabled on windows. See #284."; + } +#endif + gz::common::Console::SetVerbosity(4); + } + + // Create a BoundingBox Camera sensor from a SDF and gets a boxes message + public: void BoxesWithBuiltinSDF(const std::string &_renderEngine); + + // Create a BoundingBox Camera sensor from a SDF with empty trigger topic + public: void EmptyTriggerTopic(const std::string &_renderEngine); +}; + +/// \brief mutex for thread safety +std::mutex g_mutex; + +/// \brief bounding boxes from the camera +std::vector g_boxes; + +/// \brief callback to receive 2d boxes from the camera +void OnNewBoundingBoxes(const gz::msgs::AnnotatedAxisAligned2DBox_V &_boxes) +{ + g_mutex.lock(); + g_boxes.clear(); + + int size = _boxes.annotated_box_size(); + for (int i = 0; i < size; ++i) + { + auto annotated_box = _boxes.annotated_box(i); + g_boxes.push_back(annotated_box); + } + g_mutex.unlock(); +} + +/// \brief Build a scene with 2 visible boxes +void BuildScene(gz::rendering::ScenePtr _scene) +{ + gz::math::Vector3d box1Position(1, -1, 0); + gz::math::Vector3d box2Position(1, 1, 0); + + gz::rendering::VisualPtr root = _scene->RootVisual(); + + gz::rendering::VisualPtr box1 = _scene->CreateVisual(); + box1->AddGeometry(_scene->CreateBox()); + box1->SetOrigin(0.0, 0.0, 0.0); + box1->SetLocalPosition(box1Position); + box1->SetLocalRotation(0, 0, 0); + box1->SetUserData("label", 1); + root->AddChild(box1); + + gz::rendering::VisualPtr box2 = _scene->CreateVisual(); + box2->AddGeometry(_scene->CreateBox()); + box2->SetOrigin(0.0, 0.0, 0.0); + box2->SetLocalPosition(box2Position); + box2->SetLocalRotation(0, 0, 0); + box2->SetUserData("label", 2); + root->AddChild(box2); +} + +void TriggeredBoundingBoxCameraTest::BoxesWithBuiltinSDF( + const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "triggered_boundingbox_camera_sensor_builtin.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")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Skip unsupported engines + if (_renderEngine != "ogre2") + { + gzdbg << "Engine '" << _renderEngine + << "' doesn't support bounding box cameras" << std::endl; + return; + } + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + gzdbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + BuildScene(scene); + + gz::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + gz::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + ASSERT_NE(sensor, nullptr); + EXPECT_FALSE(sensor->HasConnections()); + sensor->SetScene(scene); + + EXPECT_EQ(320u, sensor->ImageWidth()); + EXPECT_EQ(240u, sensor->ImageHeight()); + EXPECT_EQ(true, sdfSensor.CameraSensor()->Triggered()); + + // subscribe to the BoundingBox camera topic + gz::transport::Node node; + std::string boxTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF"; + node.Subscribe(boxTopic, &OnNewBoundingBoxes); + + // we should not have image before trigger + { + std::string imageTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF_image"; + WaitForMessageTestHelper helper(imageTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_FALSE(helper.WaitForMessage(1s)) << helper; + g_mutex.lock(); + EXPECT_EQ(g_boxes.size(), size_t(0)); + g_mutex.unlock(); + } + + // trigger camera through topic + std::string triggerTopic = + "/test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF/trigger"; + + auto pub = node.Advertise(triggerTopic); + gz::msgs::Boolean msg; + msg.set_data(true); + pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + + // we should receive images and boxes after trigger + { + WaitForMessageTestHelper + helper(boxTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; + g_mutex.lock(); + EXPECT_EQ(g_boxes.size(), size_t(2)); + g_mutex.unlock(); + } + + // Clean up + mgr.Remove(sensor->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +void TriggeredBoundingBoxCameraTest::EmptyTriggerTopic( + const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "triggered_boundingbox_camera_sensor_topic_builtin.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")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Skip unsupported engines + if (_renderEngine != "ogre2") + { + gzdbg << "Engine '" << _renderEngine + << "' doesn't support bounding box cameras" << std::endl; + return; + } + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + gzdbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + ASSERT_NE(nullptr, scene); + BuildScene(scene); + + gz::sensors::Manager mgr; + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + + std::string type = sdfSensor.TypeStr(); + EXPECT_EQ(type, "boundingbox_camera"); + + gz::sensors::BoundingBoxCameraSensor *sensor = + mgr.CreateSensor(sdfSensor); + ASSERT_NE(sensor, nullptr); + EXPECT_FALSE(sensor->HasConnections()); + sensor->SetScene(scene); + + // trigger camera through topic + std::string triggerTopic = + "/test/integration/triggered_bbcamera/trigger"; + + gz::transport::Node node; + auto pub = node.Advertise(triggerTopic); + gz::msgs::Boolean msg; + msg.set_data(true); + pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + + // we should receive images and boxes after trigger + { + std::string boxTopic = + "/test/integration/triggered_bbcamera"; + WaitForMessageTestHelper + helper(boxTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; + g_mutex.lock(); + EXPECT_EQ(g_boxes.size(), size_t(2)); + g_mutex.unlock(); + } + + // Clean up + mgr.Remove(sensor->Id()); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + +////////////////////////////////////////////////// +TEST_P(TriggeredBoundingBoxCameraTest, BoxesWithBuiltinSDF) +{ + BoxesWithBuiltinSDF(GetParam()); +} + +////////////////////////////////////////////////// +TEST_P(TriggeredBoundingBoxCameraTest, EmptyTriggerTopic) +{ + EmptyTriggerTopic(GetParam()); +} + +INSTANTIATE_TEST_SUITE_P(BoundingBoxCameraSensor, + TriggeredBoundingBoxCameraTest, + RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); + +////////////////////////////////////////////////// +int main(int argc, char **argv) +{ + gz::common::Console::SetVerbosity(4); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc index 31d86be6..873fe486 100644 --- a/test/integration/triggered_camera.cc +++ b/test/integration/triggered_camera.cc @@ -63,6 +63,9 @@ class TriggeredCameraTest: public testing::Test, // Create a Camera sensor from a SDF and gets a image message public: void ImagesWithBuiltinSDF(const std::string &_renderEngine); + + // Create a Camera sensor from a SDF with empty trigger topic + public: void EmptyTriggerTopic(const std::string &_renderEngine); }; void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) @@ -128,6 +131,9 @@ void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) msg.set_data(true); pub.Publish(msg); + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + // check camera image after trigger { std::string imageTopic = @@ -154,6 +160,73 @@ void TriggeredCameraTest::ImagesWithBuiltinSDF(const std::string &_renderEngine) gz::rendering::unloadEngine(engine->Name()); } +void TriggeredCameraTest::EmptyTriggerTopic(const std::string &_renderEngine) +{ + std::string path = gz::common::joinPaths(PROJECT_SOURCE_PATH, "test", + "sdf", "triggered_camera_sensor_topic_builtin.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")); + auto sensorPtr = linkPtr->GetElement("sensor"); + + // Setup gz-rendering with an empty scene + auto *engine = gz::rendering::engine(_renderEngine); + if (!engine) + { + gzdbg << "Engine '" << _renderEngine + << "' is not supported" << std::endl; + return; + } + + gz::rendering::ScenePtr scene = engine->CreateScene("scene"); + + gz::sensors::Manager mgr; + + gz::sensors::CameraSensor *sensor = + mgr.CreateSensor(sensorPtr); + ASSERT_NE(sensor, nullptr); + EXPECT_FALSE(sensor->HasConnections()); + sensor->SetScene(scene); + + sdf::Sensor sdfSensor; + sdfSensor.Load(sensorPtr); + EXPECT_EQ(true, sdfSensor.CameraSensor()->Triggered()); + + // trigger camera through default generated topic + gz::transport::Node triggerNode; + std::string triggerTopic = + "/test/integration/triggered_camera/trigger"; + + auto pub = triggerNode.Advertise(triggerTopic); + gz::msgs::Boolean msg; + msg.set_data(true); + pub.Publish(msg); + + // sleep to wait for trigger msg to be received before calling mgr.RunOnce + std::this_thread::sleep_for(2s); + + // check camera image after trigger + { + std::string imageTopic = + "/test/integration/triggered_camera"; + WaitForMessageTestHelper helper(imageTopic); + mgr.RunOnce(std::chrono::steady_clock::duration::zero(), true); + EXPECT_TRUE(helper.WaitForMessage(10s)) << helper; + } + + // Clean up + auto sensorId = sensor->Id(); + EXPECT_TRUE(mgr.Remove(sensorId)); + engine->DestroyScene(scene); + gz::rendering::unloadEngine(engine->Name()); +} + ////////////////////////////////////////////////// TEST_P(TriggeredCameraTest, ImagesWithBuiltinSDF) { @@ -161,5 +234,12 @@ TEST_P(TriggeredCameraTest, ImagesWithBuiltinSDF) ImagesWithBuiltinSDF(GetParam()); } +////////////////////////////////////////////////// +TEST_P(TriggeredCameraTest, EmptyTriggerTopic) +{ + gz::common::Console::SetVerbosity(4); + EmptyTriggerTopic(GetParam()); +} + INSTANTIATE_TEST_SUITE_P(CameraSensor, TriggeredCameraTest, RENDER_ENGINE_VALUES, gz::rendering::PrintToStringParam()); diff --git a/test/sdf/camera_intrinsics.sdf b/test/sdf/camera_intrinsics.sdf index 3b715ac5..85557b52 100644 --- a/test/sdf/camera_intrinsics.sdf +++ b/test/sdf/camera_intrinsics.sdf @@ -47,7 +47,7 @@ 10 - base_camera + base_camera /camera3/image 1.0472 diff --git a/test/sdf/camera_projection.sdf b/test/sdf/camera_projection.sdf index 7415640a..80a0a271 100644 --- a/test/sdf/camera_projection.sdf +++ b/test/sdf/camera_projection.sdf @@ -4,40 +4,67 @@ 10 - base_camera + base_camera /camera1/image - 1.05 + 1.0472 1000 1000 - L8 + R8G8B8 10 - base_camera + base_camera /camera2/image - 1.05 + 1.0472 1000 1000 - L8 + R8G8B8 866.23 - 966.23 + 866.23 500 - 400 + 500 300 200 + + 10 + base_camera + /camera3/image + + 1.0472 + + 1000 + 1000 + R8G8B8 + + + 0.1 + 100 + + + + 900 + 900 + 501 + 501 + 0 + 0 + + + + diff --git a/test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf b/test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf new file mode 100644 index 00000000..83ae00fd --- /dev/null +++ b/test/sdf/triggered_boundingbox_camera_sensor_builtin.sdf @@ -0,0 +1,24 @@ + + + + + + 10 + /test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF + + true + /test/integration/TriggeredBBCameraPlugin_imagesWithBuiltinSDF/trigger + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + \ No newline at end of file diff --git a/test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf b/test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf new file mode 100644 index 00000000..cf6a1448 --- /dev/null +++ b/test/sdf/triggered_boundingbox_camera_sensor_topic_builtin.sdf @@ -0,0 +1,23 @@ + + + + + + 10 + /test/integration/triggered_bbcamera + + true + 1.05 + + 320 + 240 + + + 0.1 + 10.0 + + + + + + diff --git a/test/sdf/triggered_camera_sensor_topic_builtin.sdf b/test/sdf/triggered_camera_sensor_topic_builtin.sdf new file mode 100644 index 00000000..84a9943a --- /dev/null +++ b/test/sdf/triggered_camera_sensor_topic_builtin.sdf @@ -0,0 +1,23 @@ + + + + + + 10 + /test/integration/triggered_camera + + true + 1.05 + + 256 + 257 + + + 0.1 + 10.0 + + + + + +