diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index d619c1c9f2..af0b4c43fe 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -6,4 +6,5 @@ examples/* @mabelzhang src/systems/physics/* @azeey src/systems/sensors/* @iche033 +*/gui/* @jennuine tutorials/* @mabelzhang diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0206d99944..a415239685 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 - uses: actions/setup-python@v3 - uses: pre-commit/action@v3.0.0 with: @@ -25,7 +25,7 @@ jobs: name: Ubuntu Jammy CI steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v3 - uses: actions/setup-python@v3 - uses: pre-commit/action@v3.0.0 with: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index d5472e5c9f..fbeddc84bc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,6 +1,6 @@ repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.3.0 + rev: v4.4.0 hooks: - id: check-added-large-files args: ['--maxkb=500'] diff --git a/CMakeLists.txt b/CMakeLists.txt index 0bd8189539..f75b0d492e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,7 +78,7 @@ set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) #-------------------------------------- # Find gz-transport -gz_find_package(gz-transport13 REQUIRED COMPONENTS log) +gz_find_package(gz-transport13 REQUIRED COMPONENTS log parameters) set(GZ_TRANSPORT_VER ${gz-transport13_VERSION_MAJOR}) #-------------------------------------- @@ -134,6 +134,7 @@ gz_find_package(gz-sensors8 REQUIRED COMPONENTS # non-rendering air_pressure + air_speed altimeter imu force_torque @@ -187,12 +188,10 @@ set(GZ_UTILS_VER ${gz-utils2_VERSION_MAJOR}) #-------------------------------------- # Find protobuf -set(REQ_PROTOBUF_VER 3) gz_find_package(GzProtobuf - VERSION ${REQ_PROTOBUF_VER} - REQUIRED - COMPONENTS all - PRETTY Protobuf) + REQUIRED + COMPONENTS all + PRETTY Protobuf) set(Protobuf_IMPORT_DIRS ${gz-msgs10_INCLUDE_DIRS}) #-------------------------------------- diff --git a/COPYING b/COPYING deleted file mode 100644 index d9a10c0d8e..0000000000 --- a/COPYING +++ /dev/null @@ -1,176 +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 b45d4f6018..7f722f8fdd 100644 --- a/Changelog.md +++ b/Changelog.md @@ -4,7 +4,301 @@ ## Gazebo Sim 7.x -### Gazebo Sim 7.X.X (20XX-XX-XX) +### Gazebo Sim 7.5.0 (2023-05-14) + +1. Actuators message input for JointController. + * [Pull request #1953](https://github.com/gazebosim/gz-sim/pull/1953) + +1. fixed a code block in the python interfaces tutorial + * [Pull request #1982](https://github.com/gazebosim/gz-sim/pull/1982) + +1. Add missing cmake exports from core library + * [Pull request #1978](https://github.com/gazebosim/gz-sim/pull/1978) + +1. Actuators message for JointPositionController. + * [Pull request #1954](https://github.com/gazebosim/gz-sim/pull/1954) + +1. Update sdf plugins to use actuator_number. + * [Pull request #1976](https://github.com/gazebosim/gz-sim/pull/1976) + +1. Unload render engine when the sensors system exits + * [Pull request #1960](https://github.com/gazebosim/gz-sim/pull/1960) + +1. Use GzSpinBox. + * [Pull request #1969](https://github.com/gazebosim/gz-sim/pull/1969) + +1. Add tutorial on migrating the Actor class from gazebo classic + * [Pull request #1929](https://github.com/gazebosim/gz-sim/pull/1929) + +1. Add back in the marker example + * [Pull request #1972](https://github.com/gazebosim/gz-sim/pull/1972) + +1. Optimize render updates and use of thread mutexes in Sensors system + * [Pull request #1938](https://github.com/gazebosim/gz-sim/pull/1938) + +1. Fix use of actors that only has trajectory animation + * [Pull request #1947](https://github.com/gazebosim/gz-sim/pull/1947) + +1. Actuators message input for Ackermann Steering. + * [Pull request #1952](https://github.com/gazebosim/gz-sim/pull/1952) + +1. Add tutorial on migrating the Joint class from gazebo classic + * [Pull request #1925](https://github.com/gazebosim/gz-sim/pull/1925) + +1. Add tutorial on migrating the Light class from gazebo classic + * [Pull request #1931](https://github.com/gazebosim/gz-sim/pull/1931) + +1. Remove filtering from realtime factor (RTF) calculation + * [Pull request #1942](https://github.com/gazebosim/gz-sim/pull/1942) + +1. Fix docker/README.md + * [Pull request #1964](https://github.com/gazebosim/gz-sim/pull/1964) + +1. gz_TEST: improve initial sim time test reliability + * [Pull request #1916](https://github.com/gazebosim/gz-sim/pull/1916) + +1. Use a queue to track component registration from mulitiple sources + * [Pull request #1836](https://github.com/gazebosim/gz-sim/pull/1836) + +1. Initialize services in ViewAngle constructor + * [Pull request #1943](https://github.com/gazebosim/gz-sim/pull/1943) + +1. CI workflow: use checkout v3 + * [Pull request #1940](https://github.com/gazebosim/gz-sim/pull/1940) + +1. Rename COPYING to LICENSE + * [Pull request #1937](https://github.com/gazebosim/gz-sim/pull/1937) + +1. add comment on center of buoyancy force + * [Pull request #1935](https://github.com/gazebosim/gz-sim/pull/1935) + +1. Get Windows to green on gz-sim7 + * [Pull request #1917](https://github.com/gazebosim/gz-sim/pull/1917) + +1. Add Light class + * [Pull request #1918](https://github.com/gazebosim/gz-sim/pull/1918) + +1. Resolve inconsistent visibility on ign-gazebo6 + * [Pull request #1914](https://github.com/gazebosim/gz-sim/pull/1914) + +1. relax msg count check in RF comms integration test + * [Pull request #1920](https://github.com/gazebosim/gz-sim/pull/1920) + +1. Fix off-by-one error in physics test + * [Pull request #1921](https://github.com/gazebosim/gz-sim/pull/1921) + +1. Fix formatting of error messages with large mesh file names + * [Pull request #1654](https://github.com/gazebosim/gz-sim/pull/1654) + +1. Add Actor class + * [Pull request #1913](https://github.com/gazebosim/gz-sim/pull/1913) + +1. Update all velocity and acceleration components of non-link entities + * [Pull request #1868](https://github.com/gazebosim/gz-sim/pull/1868) + +1. Add Sensor class + * [Pull request #1912](https://github.com/gazebosim/gz-sim/pull/1912) + +1. Minor vocab fix + * [Pull request #1915](https://github.com/gazebosim/gz-sim/pull/1915) + +1. Allow to change camera user hfov in camera_view plugin + * [Pull request #1807](https://github.com/gazebosim/gz-sim/pull/1807) + +1. Address a few Windows CI Issues + * [Pull request #1911](https://github.com/gazebosim/gz-sim/pull/1911) + +1. Added magnetometer value based on location + * [Pull request #1907](https://github.com/gazebosim/gz-sim/pull/1907) + +1. Allow specifying initial simulation time with a CLI argument + * [Pull request #1801](https://github.com/gazebosim/gz-sim/pull/1801) + +1. Add Joint class + * [Pull request #1910](https://github.com/gazebosim/gz-sim/pull/1910) + +1. Added reset simulation tutorial + * [Pull request #1824](https://github.com/gazebosim/gz-sim/pull/1824) + +1. Add SensorTopic component to rendering sensors + * [Pull request #1908](https://github.com/gazebosim/gz-sim/pull/1908) + +1. Use a queue to track component registration from mulitiple sources + * [Pull request #1836](https://github.com/gazebosim/gz-sim/pull/1836) + +1. Document behaviour changes introduced #1784 + * [Pull request #1888](https://github.com/gazebosim/gz-sim/pull/1888) + +1. Fix GUI_clean_exit test by increasing thread delay + * [Pull request #1902](https://github.com/gazebosim/gz-sim/pull/1902) + +1. Partial backport of 1728 + * [Pull request #1901](https://github.com/gazebosim/gz-sim/pull/1901) + +1. Fix gz plugin paths in windows + * [Pull request #1899](https://github.com/gazebosim/gz-sim/pull/1899) + +1. Increase timeout for UNIT_Gui_clean_exit_TEST + * [Pull request #1897](https://github.com/gazebosim/gz-sim/pull/1897) + +1. fix triggered camera test by waiting for rendering / scene to be ready + * [Pull request #1895](https://github.com/gazebosim/gz-sim/pull/1895) + +1. cmdsim.rb: fix ruby syntax + * [Pull request #1884](https://github.com/gazebosim/gz-sim/pull/1884) + +1. Fix some windows warnings (C4244 and C4305) + * [Pull request #1874](https://github.com/gazebosim/gz-sim/pull/1874) + +1. Minor optimization to transform control tool + * [Pull request #1854](https://github.com/gazebosim/gz-sim/pull/1854) + +1. inherit material cast shadows property + * [Pull request #1856](https://github.com/gazebosim/gz-sim/pull/1856) + +1. fix record topic + * [Pull request #1855](https://github.com/gazebosim/gz-sim/pull/1855) + +1. Remove duplicate Fuel server used by ResourceSpawner + * [Pull request #1830](https://github.com/gazebosim/gz-sim/pull/1830) + +1. re-add namespace + * [Pull request #1826](https://github.com/gazebosim/gz-sim/pull/1826) + +1. Fix QML warnings regarding binding loops + * [Pull request #1829](https://github.com/gazebosim/gz-sim/pull/1829) + +1. Update documentation on `UpdateInfo::realTime` + * [Pull request #1817](https://github.com/gazebosim/gz-sim/pull/1817) + +1. Add jennuine as GUI codeowner + * [Pull request #1800](https://github.com/gazebosim/gz-sim/pull/1800) + +1. remove PlotIcon + * [Pull request #1658](https://github.com/gazebosim/gz-sim/pull/1658) + +1. Final update of ignitionrobotics to gazebosim for citadel + * [Pull request #1760](https://github.com/gazebosim/gz-sim/pull/1760) + +1. Convert ignitionrobotics to gazebosim in tutorials + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1759) + +1. Convert ignitionrobotics to gazebosim in sources and includes + * [Pull request #1758](https://github.com/gazebosim/gz-sim/pull/1758) + +1. Convert ignitionrobotics to gazebosim in tests directory + * [Pull request #1757](https://github.com/gazebosim/gz-sim/pull/1757) + +1. Added collection name to About Dialog + * [Pull request #1756](https://github.com/gazebosim/gz-sim/pull/1756) + +1. Citadel: Removed warnings + * [Pull request #1753](https://github.com/gazebosim/gz-sim/pull/1753) + +1. Remove actors from screen when they are supposed to + * [Pull request #1699](https://github.com/gazebosim/gz-sim/pull/1699) + +1. readd namespaces for Q_ARGS + * [Pull request #1670](https://github.com/gazebosim/gz-sim/pull/1670) + +1. 🎈 3.14.0 + * [Pull request #1657](https://github.com/gazebosim/gz-sim/pull/1657) + +1. Remove redundant namespace references + * [Pull request #1635](https://github.com/gazebosim/gz-sim/pull/1635) + +1. 🎈 3.14.0~pre1 + * [Pull request #1650](https://github.com/gazebosim/gz-sim/pull/1650) + +### Gazebo Sim 7.4.0 (2023-02-14) + +1. Added airspeed sensor + * [Pull request #1847](https://github.com/gazebosim/gz-sim/pull/1847) + +1. JointPosController: support nested joints + * [Pull request #1851](https://github.com/gazebosim/gz-sim/pull/1851) + +1. cmdsim.rb: fix ruby syntax + * [Pull request #1884](https://github.com/gazebosim/gz-sim/pull/1884) + +1. Fix view angle plugin + * [Pull request #1877](https://github.com/gazebosim/gz-sim/pull/1877) + +1. Fix cmake unrecognized argument warning + * [Pull request #1882](https://github.com/gazebosim/gz-sim/pull/1882) + +### Gazebo Sim 7.3.0 (2023-02-02) + +1. Fluid added mass + * [Pull request #1592](https://github.com/gazebosim/gz-sim/pull/1592) + +1. Add P gain value for Ackermann steering. + * [Pull request #1873](https://github.com/gazebosim/gz-sim/pull/1873) + +1. Add orientation to Odom with covariance. + * [Pull request #1876](https://github.com/gazebosim/gz-sim/pull/1876) + +### Gazebo Sim 7.2.0 (2023-01-25) + +1. Enable the JointController and JointPositionController to use sub_topics and control multiple joints. + * [Pull request #1861](https://github.com/gazebosim/gz-sim/pull/1861) + +1. Ackermann steering with steering angle and sub_topic. + * [Pull request #1860](https://github.com/gazebosim/gz-sim/pull/1860) + +1. port: 6 to 7 (10-JAN-2023) + * [Pull request #1857](https://github.com/gazebosim/gz-sim/pull/1857) + +1. Add ignition alias back + * [Pull request #1858](https://github.com/gazebosim/gz-sim/pull/1858) + +1. fix SdfGenerator unit test + * [Pull request #1853](https://github.com/gazebosim/gz-sim/pull/1853) + +1. Allow using a CSV file to define currents for hydrodynamic system + * [Pull request #1839](https://github.com/gazebosim/gz-sim/pull/1839) + +1. Add multichannel lookup for environment sensors. + * [Pull request #1814](https://github.com/gazebosim/gz-sim/pull/1814) + +1. Example controller for LRAUV + * [Pull request #1822](https://github.com/gazebosim/gz-sim/pull/1822) + +1. Fix component removal in component inspector + * [Pull request #1833](https://github.com/gazebosim/gz-sim/pull/1833) + +1. port: 6 to 7 (06-DEC-2023) + * [Pull request #1832](https://github.com/gazebosim/gz-sim/pull/1832) + +1. port: 6 to 7 (29-NOV-2023) + * [Pull request #1821](https://github.com/gazebosim/gz-sim/pull/1821) + +1. Fix #1812. + * [Pull request #1813](https://github.com/gazebosim/gz-sim/pull/1813) + +1. Removed unused attributes + * [Pull request #1809](https://github.com/gazebosim/gz-sim/pull/1809) + +1. Fixes buoyancy flakiness when spawning entities + * [Pull request #1808](https://github.com/gazebosim/gz-sim/pull/1808) + +1. Remove fixed width from world control + * [Pull request #1805](https://github.com/gazebosim/gz-sim/pull/1805) + +1. Backport #1748: Adds a tool for environment data visualization and custom environmental sensors + * [Pull request #1798](https://github.com/gazebosim/gz-sim/pull/1798) + +1. Acoustic comms : Propagation model + * [Pull request #1793](https://github.com/gazebosim/gz-sim/pull/1793) + +1. Add pre-commit hooks configuration + * [Pull request #1792](https://github.com/gazebosim/gz-sim/pull/1792) + +1. Add checkbox in view angle plugin for toggling view control reference visual + * [Pull request #1788](https://github.com/gazebosim/gz-sim/pull/1788) + +1. Add EnvironmentalData component + * [Pull request #1616](https://github.com/gazebosim/gz-sim/pull/1616) ### Gazebo Sim 7.1.0 (2022-11-10) @@ -437,6 +731,60 @@ ## Gazebo Sim 6.x +### Gazebo Sim 6.14.0 (2022-12-29) + +1. Fix Ackermann plugin zero linVel turningRadius bug + * [Pull request #1849](https://github.com/gazebosim/gz-sim/pull/1849) + +1. Header guard fix for battery power load component + * [Pull request #1846](https://github.com/gazebosim/gz-sim/pull/1846) + +1. Add interface to allow systems to declare parameters + * [Pull request #1431](https://github.com/gazebosim/gz-sim/pull/1431) + +1. Adding battery consumers and extra fixes + * [Pull request #1811](https://github.com/gazebosim/gz-sim/pull/1811) + +1. Disable tests that require dartsim on windows + * [Pull request #1840](https://github.com/gazebosim/gz-sim/pull/1840) + +1. Added move camera to model service + * [Pull request #1823](https://github.com/gazebosim/gz-sim/pull/1823) + +1. Add spin box to View Angle plugin for configuring view control sensitivity + * [Pull request #1799](https://github.com/gazebosim/gz-sim/pull/1799) + +1. Sync View Angle GUI with view controller + * [Pull request #1825](https://github.com/gazebosim/gz-sim/pull/1825) + +1. Hydrodynamics flags test strengthening + * [Pull request #1819](https://github.com/gazebosim/gz-sim/pull/1819) + +1. Fixed Fortress tests related to lights + * [Pull request #1827](https://github.com/gazebosim/gz-sim/pull/1827) + +1. Allow to move to model from Angle view plugin + * [Pull request #1810](https://github.com/gazebosim/gz-sim/pull/1810) + +1. Fixed light entity number + * [Pull request #1818](https://github.com/gazebosim/gz-sim/pull/1818) + +1. Check AddBvnAnimation return value + * [Pull request #1750](https://github.com/gazebosim/gz-sim/pull/1750) + +1. Add checkbox in view angle plugin for toggling view control reference visual + * [Pull request #1788](https://github.com/gazebosim/gz-sim/pull/1788) + +1. Adds support for hydrodynamic cross terms + * [Pull request #1784](https://github.com/gazebosim/gz-sim/pull/1784) + +1. Addresses flakiness in `Hydrodynamics.VelocityTestInOil`. + * [Pull request #1787](https://github.com/gazebosim/gz-sim/pull/1787) + +1. Fix minor bugs in RFComms plugin + * [Pull request #1743](https://github.com/gazebosim/gz-sim/pull/1743) + + ### Gazebo Sim 6.13.0 (2022-11-04) 1. Fix two tests on Windows diff --git a/LICENSE b/LICENSE index 69fc1e1045..d9a10c0d8e 100644 --- a/LICENSE +++ b/LICENSE @@ -1,15 +1,176 @@ -Software License Agreement (Apache License) + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ -Copyright 2018 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. - http://www.apache.org/licenses/LICENSE-2.0 + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. -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. + "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/docker/README.md b/docker/README.md index c0c2fc0d98..b641ee83bb 100644 --- a/docker/README.md +++ b/docker/README.md @@ -70,7 +70,7 @@ distribution using debians. 1. Build a docker image using the `build.bash` command. The first argument must be the name of the Gazebo distribution. The list of Gazebo distribution can be found at [Gazebo distribution](https://gazebosim.org/docs). For example, to build an - image of Gazebo Fortress: + image of Gazebo Garden: ``` ./build.bash gz-garden ./Dockerfile.gz diff --git a/examples/plugin/system_plugin/SampleSystem.hh b/examples/plugin/system_plugin/SampleSystem.hh index b5fd49799b..be38c4defb 100644 --- a/examples/plugin/system_plugin/SampleSystem.hh +++ b/examples/plugin/system_plugin/SampleSystem.hh @@ -45,7 +45,8 @@ namespace sample_system // and ISystemPostUpdate interfaces. public gz::sim::ISystemPreUpdate, public gz::sim::ISystemUpdate, - public gz::sim::ISystemPostUpdate + public gz::sim::ISystemPostUpdate, + public gz::sim::ISystemReset { public: SampleSystem2(); @@ -59,6 +60,9 @@ namespace sample_system public: void PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) override; + + public: void Reset(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) override; }; } //! [header] diff --git a/examples/plugin/system_plugin/SampleSystem2.cc b/examples/plugin/system_plugin/SampleSystem2.cc index 2def3fb4a7..ec8c9f02fb 100644 --- a/examples/plugin/system_plugin/SampleSystem2.cc +++ b/examples/plugin/system_plugin/SampleSystem2.cc @@ -8,7 +8,8 @@ GZ_ADD_PLUGIN( gz::sim::System, sample_system::SampleSystem2::ISystemPreUpdate, sample_system::SampleSystem2::ISystemUpdate, - sample_system::SampleSystem2::ISystemPostUpdate) + sample_system::SampleSystem2::ISystemPostUpdate, + sample_system::SampleSystem2::ISystemReset) //! [registerSampleSystem2] using namespace sample_system; @@ -38,3 +39,9 @@ void SampleSystem2::PostUpdate(const gz::sim::UpdateInfo &_info, { gzmsg << "SampleSystem2::PostUpdate" << std::endl; } + +void SampleSystem2::Reset(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + gzmsg << "SampleSystem2::Reset" << std::endl; +} diff --git a/examples/standalone/lrauv_control/CMakeLists.txt b/examples/standalone/lrauv_control/CMakeLists.txt new file mode 100644 index 0000000000..f0cbc94a03 --- /dev/null +++ b/examples/standalone/lrauv_control/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(gz-sim-lrauv-control) + +find_package(gz-transport13 QUIET REQUIRED OPTIONAL_COMPONENTS log) +set(GZ_TRANSPORT_VER ${gz-transport13_VERSION_MAJOR}) + +find_package(gz-sim8 REQUIRED) +set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) + +add_executable(lrauv_control lrauv_control.cc) +target_link_libraries(lrauv_control + gz-transport${GZ_TRANSPORT_VER}::core + gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER}) diff --git a/examples/standalone/lrauv_control/README.md b/examples/standalone/lrauv_control/README.md new file mode 100644 index 0000000000..2b64531795 --- /dev/null +++ b/examples/standalone/lrauv_control/README.md @@ -0,0 +1,55 @@ +## Introduction +This example shows a simple controller for an LRAUV. The vehicle has 3 actuators : the thruster, the fins to control yaw angle, and the fins to control +pitch angle. Here, the target state of the vehicle is the speed, yaw and pitch angle, and the controller must adjust the fin angles and thruster to achieve +that state. The vehicle is a MIMO system in reality, where all the three actuators affect each other, but we can approximate this to be a SISO system when +the pich and yaw angles to be changed are not too drastic. + +Therefore, we apply a PD controller for speed, and a P controller for yaw and pitch angles. The odometry publisher plugin supplies the required feedback +on the states. + +## Usage +This example needs to be run in two steps. + +Step 1 : Assuming ``~/gazebo_ws/`` is your Gazebo workspace, find the world file ``~/gazebo_ws/src/gz-sim/examples/worlds/lrauv_control_demo.sdf``. + +Open a new terminal window, source your gazebo workspace and run : +``` +cd ~/gazebo_ws/src/gz-sim/examples/worlds +gz sim -r lrauv_control_demo.sdf +``` + +This should open up a new gazebo window with the LRAUV at rest at the origin. + +Step 2 : Open a new terminal, source your gazebo workspace, and navigate to this example. + +``` +cd ~/gazebo_ws/src/gz-sim/examples/standalone/lrauv_control +mkdir build; cd build +cmake ..; make + +# Your controller executable should be built successfully +# Usage : +# ./lrauv_control speed_in_metres_per_sec yaw_angle_in_rad pitch_angle_in_rad +./lrauv_control 0.5 0.174 0.174 +``` + +The vehicle should now move, tracing its path along the way. The console should show the errors for each state. + +``` +. +. +. +----------------------- +States ( target, current, error) : +Speed (m/s) : 0.50000 0.54543 -0.04543 +Yaw angle (deg) : 9.96947 11.31291 -1.34344 +Pitch angle (deg) : 9.96947 6.39717 3.57229 +----------------------- +States ( target, current, error) : +Speed (m/s) : 0.50000 0.54543 -0.04543 +Yaw angle (deg) : 9.96947 11.31291 -1.34344 +Pitch angle (deg) : 9.96947 6.39717 3.57229 +``` + +## Note +This is only meant to be an example, and the controller might not behave correctly at high speeds or sudden change in desired angles. diff --git a/examples/standalone/lrauv_control/lrauv_control.cc b/examples/standalone/lrauv_control/lrauv_control.cc new file mode 100644 index 0000000000..0ec69a81ac --- /dev/null +++ b/examples/standalone/lrauv_control/lrauv_control.cc @@ -0,0 +1,224 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +/* + * Check README for detailed instructions. + * Usage: + * $ gz sim -r worlds/lrauv_control_demo.sdf + * $ # ./lrauv_control speed_m_s yaw_rad pitch_rad + * $ ./lrauv_control 0.5 0.78 0.174 + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + + +using namespace gz; + +// Helper class for the PID controller for +// linear speed, pitch and yaw angles. +class Controller +{ + public: + // Mutex to synchronize internal variables. + std::mutex controllerMutex; + + // Desired state. + double targetSpeed = 0; + double targetYawAngle = 0; + double targetPitchAngle = 0; + + // Errors + double errorSpeed = 0; + double errorSpeedIntegral = 0; + double errorYawAngle = 0; + double errorPitchAngle = 0; + + // States to be tracked and controlled. + double speed; + double yawAngle; + double pitchAngle; + + // PID gains and error limits. + // PI for speed. + double kSpeed = -30; + double kISpeed = -0.5; + double errorSpeedIntegralMax = 10; + + // P for yaw and pitch control. + double kYawAngle = -0.5; + double kPitchAngle = 0.6; + + // Set the target states to be tracked, + // i.e. linear speed (m/s), pitch and yaw angles (rad). + void SetTargets(double _speed, + double _yaw, double _pitch) + { + std::lock_guard lock(controllerMutex); + if (_speed == 0 && + (_yaw != 0 || _pitch != 0)) + { + std::cout << "Speed needs to be non zero for non zero" + " pitch and yaw angles" << std::endl; + return; + } + + targetSpeed = _speed; + targetYawAngle = _yaw; + targetPitchAngle = _pitch; + } + + // Update the state of the vehicle. + void UpdateState(double _speed, + double _yaw, double _pitch) + { + std::lock_guard lock(controllerMutex); + speed = _speed; + yawAngle = _yaw; + pitchAngle = _pitch; + + errorSpeed = targetSpeed - speed; + errorSpeedIntegral = + std::min(errorSpeedIntegral + errorSpeed, errorSpeedIntegralMax); + errorYawAngle = targetYawAngle - yawAngle; + errorPitchAngle = targetPitchAngle - pitchAngle; + } + + // Generate control input to be applied to the thruster. + double SpeedControl() + { + return errorSpeed * kSpeed + errorSpeedIntegral * kISpeed; + } + + // Generate control input to be supplied to the yaw rudders. + double YawControl() + { + return errorYawAngle * kYawAngle; + } + + // Generate control input to be supplied to the pitch rudders. + double PitchControl() + { + return errorPitchAngle * kPitchAngle; + } +}; + +int main(int argc, char** argv) +{ + auto control = Controller(); + + if (argc == 4) + { + double targetSpeed = std::stod(argv[1]); + double targetYaw = std::stod(argv[2]); + double targetPitch = std::stod(argv[3]); + + // Target state : speed (m/s), yaw angle, pitch angle (rad). + control.SetTargets(targetSpeed ,targetYaw ,targetPitch); + } + + transport::Node node; + + // Propeller command publisher. + auto propellerTopicTethys = + gz::transport::TopicUtils::AsValidTopic( + "/model/tethys/joint/propeller_joint/cmd_thrust"); + auto propellerPubTethys = + node.Advertise(propellerTopicTethys); + + // Subscriber for vehicle pose. + std::function cbPos = + [&](const msgs::Odometry &_msg) + { + auto orientation = _msg.pose().orientation(); + + gz::math::Quaterniond q( + orientation.w(), + orientation.x(), + orientation.y(), + orientation.z()); + + // Get the yaw and pitch angles. + auto rpy = q.Euler(); + + // Get the velocity of the vehicle. + gz::math::Vector3d velocity( + _msg.twist().linear().x(), + _msg.twist().linear().y(), + _msg.twist().linear().z()); + + control.UpdateState(velocity.Length(), rpy[2], rpy[1]); + }; + + node.Subscribe("/model/tethys/odometry", cbPos); + + // Rudder command publisher. + auto rudderTopicTethys = + gz::transport::TopicUtils::AsValidTopic( + "/model/tethys/joint/vertical_fins_joint/0/cmd_pos"); + auto rudderPubTethys = + node.Advertise(rudderTopicTethys); + + // Fin command publisher. + auto finTopicTethys = + gz::transport::TopicUtils::AsValidTopic( + "/model/tethys/joint/horizontal_fins_joint/0/cmd_pos"); + auto finPubTethys = + node.Advertise(finTopicTethys); + + while (true) + { + std::this_thread::sleep_for(std::chrono::milliseconds(200)); + + std::lock_guard lock(control.controllerMutex); + // Publish propeller command for speed. + msgs::Double propellerMsg; + propellerMsg.set_data(control.SpeedControl()); + propellerPubTethys.Publish(propellerMsg); + + // Publish rudder command for yaw. + msgs::Double rudderMsg; + rudderMsg.set_data(control.YawControl()); + rudderPubTethys.Publish(rudderMsg); + + // Publish fin command for pitch. + msgs::Double finMsg; + finMsg.set_data(control.PitchControl()); + finPubTethys.Publish(finMsg); + + // Print the states. + std::cout << std::setprecision(5) << std::fixed; + std::cout << "-----------------------" << std::endl; + std::cout << "States ( target, current, error) : " << std::endl; + + std::cout << "Speed (m/s) : " << control.targetSpeed << " " << control.speed << " " + << control.errorSpeed << std::endl; + + std::cout << "Yaw angle (deg) : " << control.targetYawAngle * 180/GZ_PI << " " + << control.yawAngle * 180/GZ_PI << " " << control.errorYawAngle * 180/GZ_PI<< std::endl; + + std::cout << "Pitch angle (deg) : "<< control.targetPitchAngle * 180/GZ_PI << " " + << control.pitchAngle * 180/GZ_PI << " " << control.errorPitchAngle * 180/GZ_PI<< std::endl; + } +} diff --git a/examples/standalone/marker/CMakeLists.txt b/examples/standalone/marker/CMakeLists.txt new file mode 100644 index 0000000000..bbca4e481d --- /dev/null +++ b/examples/standalone/marker/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) + +project(gz-sim-marker) + +if(${CMAKE_SYSTEM_NAME} STREQUAL "Linux") + find_package(gz-transport13 QUIET REQUIRED) + set(GZ_TRANSPORT_VER ${gz-transport13_VERSION_MAJOR}) + + find_package(gz-common5 REQUIRED) + set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) + + find_package(gz-msgs10 REQUIRED) + set(GZ_MSGS_VER ${gz-msgs10_VERSION_MAJOR}) + + add_executable(marker marker.cc) + target_link_libraries(marker + gz-transport${GZ_TRANSPORT_VER}::core + gz-msgs${GZ_MSGS_VER} + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + ) +endif() diff --git a/examples/standalone/marker/README.md b/examples/standalone/marker/README.md new file mode 100644 index 0000000000..83a6c367f7 --- /dev/null +++ b/examples/standalone/marker/README.md @@ -0,0 +1,22 @@ +# Gazebo Visualization Marker Example + +This example demonstrates how to create, modify, and delete visualization +markers in Gazebo. + +## Build Instructions + +From this directory: + + mkdir build + cd build + cmake .. + make + +## Execute Instructions + +Launch `gz sim` unpaused then from the build directory above: + + ./marker + +The terminal will output messages indicating visualization changes that +will occur in Gazebo's render window. diff --git a/examples/standalone/marker/marker.cc b/examples/standalone/marker/marker.cc new file mode 100644 index 0000000000..5cf7e725ba --- /dev/null +++ b/examples/standalone/marker/marker.cc @@ -0,0 +1,314 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include +#include +#include + +#include + +///////////////////////////////////////////////// +int main(int _argc, char **_argv) +{ + gz::transport::Node node; + + // Create the marker message + gz::msgs::Marker markerMsg; + gz::msgs::Material matMsg; + markerMsg.set_ns("default"); + markerMsg.set_id(0); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::SPHERE); + markerMsg.set_visibility(gz::msgs::Marker::GUI); + + // Set color to Blue + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(1); + markerMsg.mutable_material()->mutable_ambient()->set_a(1); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(1); + markerMsg.mutable_material()->mutable_diffuse()->set_a(1); + markerMsg.mutable_lifetime()->set_sec(2); + markerMsg.mutable_lifetime()->set_nsec(0); + gz::msgs::Set(markerMsg.mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + + // The rest of this function adds different shapes and/or modifies shapes. + // Read the terminal statements to figure out what each node.Request + // call accomplishes. + std::cout << "Spawning a blue sphere with lifetime 2s\n"; + GZ_SLEEP_S(4); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(2, 2, 0, 0, 0, 0)); + node.Request("/marker", markerMsg); + std::cout << "Sleeping for 2 seconds\n"; + GZ_SLEEP_S(2); + + std::cout << "Spawning a black sphere at the origin with no lifetime\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(1); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d::Zero); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + markerMsg.mutable_lifetime()->set_sec(0); + node.Request("/marker", markerMsg); + + std::cout << "Moving the sphere to x=0, y=1, z=1\n"; + GZ_SLEEP_S(4); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, 1, 1, 0, 0, 0)); + node.Request("/marker", markerMsg); + + std::cout << "Shrinking the sphere\n"; + GZ_SLEEP_S(4); + gz::msgs::Set(markerMsg.mutable_scale(), + gz::math::Vector3d(0.2, 0.2, 0.2)); + node.Request("/marker", markerMsg); + + std::cout << "Changing the sphere to red\n"; + markerMsg.mutable_material()->mutable_ambient()->set_r(1); + markerMsg.mutable_material()->mutable_ambient()->set_g(0); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_r(1); + markerMsg.mutable_material()->mutable_diffuse()->set_g(0); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + GZ_SLEEP_S(4); + node.Request("/marker", markerMsg); + + std::cout << "Adding a green box\n"; + markerMsg.mutable_material()->mutable_ambient()->set_r(0); + markerMsg.mutable_material()->mutable_ambient()->set_g(1); + markerMsg.mutable_material()->mutable_ambient()->set_b(0); + markerMsg.mutable_material()->mutable_diffuse()->set_r(0); + markerMsg.mutable_material()->mutable_diffuse()->set_g(1); + markerMsg.mutable_material()->mutable_diffuse()->set_b(0); + GZ_SLEEP_S(4); + markerMsg.set_id(2); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::BOX); + gz::msgs::Set(markerMsg.mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(2, 0, .5, 0, 0, 0)); + node.Request("/marker", markerMsg); + + std::cout << "Changing the green box to a cylinder\n"; + GZ_SLEEP_S(4); + markerMsg.set_type(gz::msgs::Marker::CYLINDER); + node.Request("/marker", markerMsg); + + std::cout << "Connecting the sphere and cylinder with a line\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(3); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::LINE_LIST); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(2, 0, 0.5)); + node.Request("/marker", markerMsg); + + std::cout << "Adding a square around the origin\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(4); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::LINE_STRIP); + gz::msgs::Set(markerMsg.mutable_point(0), + gz::math::Vector3d(0.5, 0.5, 0.05)); + gz::msgs::Set(markerMsg.mutable_point(1), + gz::math::Vector3d(0.5, -0.5, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(-0.5, -0.5, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(-0.5, 0.5, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0.5, 0.5, 0.05)); + node.Request("/marker", markerMsg); + + std::cout << "Adding 100 points inside the square\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(5); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::POINTS); + markerMsg.clear_point(); + for (int i = 0; i < 100; ++i) + { + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d( + gz::math::Rand::DblUniform(-0.49, 0.49), + gz::math::Rand::DblUniform(-0.49, 0.49), + 0.05)); + } + node.Request("/marker", markerMsg); + + std::cout << "Adding a semi-circular triangle fan\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(6); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::TRIANGLE_FAN); + markerMsg.clear_point(); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, 1.5, 0, 0, 0, 0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 0, 0.05)); + double radius = 2; + for (double t = 0; t <= M_PI; t+= 0.01) + { + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(radius * cos(t), radius * sin(t), 0.05)); + } + node.Request("/marker", markerMsg); + + std::cout << "Adding two triangles using a triangle list\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(7); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::TRIANGLE_LIST); + markerMsg.clear_point(); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(0, -1.5, 0, 0, 0, 0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 1, 0.05)); + + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 1, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(2, 1, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(2, 2, 0.05)); + + node.Request("/marker", markerMsg); + + std::cout << "Adding a rectangular triangle strip\n"; + GZ_SLEEP_S(4); + markerMsg.set_id(8); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::TRIANGLE_STRIP); + markerMsg.clear_point(); + gz::msgs::Set(markerMsg.mutable_pose(), + gz::math::Pose3d(-2, -2, 0, 0, 0, 0)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 0, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 1, 0.05)); + + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 1, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(0, 2, 0.05)); + gz::msgs::Set(markerMsg.add_point(), + gz::math::Vector3d(1, 2, 0.05)); + + node.Request("/marker", markerMsg); + std::cout << "Adding multiple markers via /marker_array\n"; + GZ_SLEEP_S(4); + + gz::msgs::Marker_V markerMsgs; + gz::msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + + // Create first blue sphere marker + auto markerMsg1 = markerMsgs.add_marker(); + markerMsg1->set_ns("default"); + markerMsg1->set_id(0); + markerMsg1->set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg1->set_type(gz::msgs::Marker::SPHERE); + markerMsg1->set_visibility(gz::msgs::Marker::GUI); + + // Set color to Blue + markerMsg1->mutable_material()->mutable_ambient()->set_r(0); + markerMsg1->mutable_material()->mutable_ambient()->set_g(0); + markerMsg1->mutable_material()->mutable_ambient()->set_b(1); + markerMsg1->mutable_material()->mutable_ambient()->set_a(1); + markerMsg1->mutable_material()->mutable_diffuse()->set_r(0); + markerMsg1->mutable_material()->mutable_diffuse()->set_g(0); + markerMsg1->mutable_material()->mutable_diffuse()->set_b(1); + markerMsg1->mutable_material()->mutable_diffuse()->set_a(1); + gz::msgs::Set(markerMsg1->mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg1->mutable_pose(), + gz::math::Pose3d(3, 3, 0, 0, 0, 0)); + + // Create second red box marker + auto markerMsg2 = markerMsgs.add_marker(); + markerMsg2->set_ns("default"); + markerMsg2->set_id(0); + markerMsg2->set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg2->set_type(gz::msgs::Marker::BOX); + markerMsg2->set_visibility(gz::msgs::Marker::GUI); + + // Set color to Red + markerMsg2->mutable_material()->mutable_ambient()->set_r(1); + markerMsg2->mutable_material()->mutable_ambient()->set_g(0); + markerMsg2->mutable_material()->mutable_ambient()->set_b(0); + markerMsg2->mutable_material()->mutable_ambient()->set_a(1); + markerMsg2->mutable_material()->mutable_diffuse()->set_r(1); + markerMsg2->mutable_material()->mutable_diffuse()->set_g(0); + markerMsg2->mutable_material()->mutable_diffuse()->set_b(0); + markerMsg2->mutable_material()->mutable_diffuse()->set_a(1); + markerMsg2->mutable_lifetime()->set_sec(2); + markerMsg2->mutable_lifetime()->set_nsec(0); + gz::msgs::Set(markerMsg2->mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg2->mutable_pose(), + gz::math::Pose3d(3, 3, 2, 0, 0, 0)); + + // Create third green cylinder marker + auto markerMsg3 = markerMsgs.add_marker(); + markerMsg3->set_ns("default"); + markerMsg3->set_id(0); + markerMsg3->set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg3->set_type(gz::msgs::Marker::CYLINDER); + markerMsg3->set_visibility(gz::msgs::Marker::GUI); + + // Set color to Green + markerMsg3->mutable_material()->mutable_ambient()->set_r(0); + markerMsg3->mutable_material()->mutable_ambient()->set_g(1); + markerMsg3->mutable_material()->mutable_ambient()->set_b(0); + markerMsg3->mutable_material()->mutable_ambient()->set_a(1); + markerMsg3->mutable_material()->mutable_diffuse()->set_r(0); + markerMsg3->mutable_material()->mutable_diffuse()->set_g(1); + markerMsg3->mutable_material()->mutable_diffuse()->set_b(0); + markerMsg3->mutable_material()->mutable_diffuse()->set_a(1); + markerMsg3->mutable_lifetime()->set_sec(2); + markerMsg3->mutable_lifetime()->set_nsec(0); + gz::msgs::Set(markerMsg3->mutable_scale(), + gz::math::Vector3d(1.0, 1.0, 1.0)); + gz::msgs::Set(markerMsg3->mutable_pose(), + gz::math::Pose3d(3, 3, 4, 0, 0, 0)); + + // Publish the three created markers above simultaneously + node.Request("/marker_array", markerMsgs, timeout, res, result); + + std::cout << "Deleting all the markers\n"; + GZ_SLEEP_S(4); + markerMsg.set_action(gz::msgs::Marker::DELETE_ALL); + node.Request("/marker", markerMsg); +} diff --git a/examples/worlds/auv_controls.sdf b/examples/worlds/auv_controls.sdf index cf3618b15a..18914a6629 100644 --- a/examples/worlds/auv_controls.sdf +++ b/examples/worlds/auv_controls.sdf @@ -147,17 +147,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 diff --git a/examples/worlds/camera_sensor.sdf b/examples/worlds/camera_sensor.sdf index ce8eec9241..89a71652a8 100644 --- a/examples/worlds/camera_sensor.sdf +++ b/examples/worlds/camera_sensor.sdf @@ -15,7 +15,7 @@ - ogre + ogre2 + + + + + + + + + + + sphere_no_added_mass + model + 1 0 0 + + + + sphere_in_water + model + 1 0 0 + + + + sphere_in_air + model + 1 0 0 + + + + box_diagonal + model + 1 0 0 + + + + box_xx_xy_xz + model + 1 0 0 + + + + box_xy + model + 1 0 0 + + + + + 0 0 0 + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 0 0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 1 0 1 + 0 1 0 1 + 0 1 0 1 + + + + + + + 0 1.5 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + 261.666 + 261.666 + 261.666 + + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 0 1 1 + 0 0 1 1 + 0 0 1 1 + + + + + + + 0 -1.5 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + + 0.261666 + 0.261666 + 0.261666 + + + + + + 0.5 + + + + + + + + 0.5 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 4.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + 0.261666 + 0.261666 + + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 6.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + 0.261666 + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 8.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + diff --git a/examples/worlds/lrauv_control_demo.sdf b/examples/worlds/lrauv_control_demo.sdf new file mode 100644 index 0000000000..bf0d43a12d --- /dev/null +++ b/examples/worlds/lrauv_control_demo.sdf @@ -0,0 +1,277 @@ + + + + + + 0.0 1.0 1.0 + 0.0 0.7 0.8 + + + + 0.001 + 1.0 + + + + + + + + + + + + 1000 + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 5 -1.5 3 0 0.37 2.8 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + false + 5 + 5 + floating + false + + + + + + + + + + LRAUV path plot + + tethys::base_link + 1 0 0 + + + + + 0 0 1 0 0 0 + https://fuel.gazebosim.org/1.0/accurrent/models/MBARI Tethys LRAUV + + + + + + + horizontal_fins_joint + 0.1 + + + + vertical_fins_joint + 0.1 + + + + tethys + 0 + propeller_joint + 0.004422 + 1000 + 0.2 + + + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 1 0 + 1 0 0 + vertical_fins + 0 0 0 + + + + + 1000 + 4.13 + -1.1 + 0.2 + 0.03 + 0.17 + 0 + 0.0244 + 0 0 1 + 1 0 0 + horizontal_fins + 0 0 0 + + + + base_link + -4.876161 + -126.324739 + -126.324739 + 0 + -33.46 + -33.46 + -6.2282 + 0 + -601.27 + 0 + -601.27 + 0 + -0.1916 + 0 + -632.698957 + 0 + -632.698957 + 0 + + + + + + diff --git a/examples/worlds/multi_lrauv_race.sdf b/examples/worlds/multi_lrauv_race.sdf index abe5264e2e..f0827cd82c 100644 --- a/examples/worlds/multi_lrauv_race.sdf +++ b/examples/worlds/multi_lrauv_race.sdf @@ -139,17 +139,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 @@ -233,17 +233,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 @@ -327,17 +327,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 diff --git a/examples/worlds/multicopter_velocity_control.sdf b/examples/worlds/multicopter_velocity_control.sdf index 8bbd2f8a5d..e34d2681ff 100644 --- a/examples/worlds/multicopter_velocity_control.sdf +++ b/examples/worlds/multicopter_velocity_control.sdf @@ -117,7 +117,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -137,7 +137,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -157,7 +157,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -177,7 +177,7 @@ You can use the velocity controller and command linear velocity and yaw angular 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 @@ -247,7 +247,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 0 + 0 2.0673e-04 0 0 @@ -267,7 +267,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 1 + 1 2.0673e-04 0 0 @@ -287,7 +287,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 2 + 2 2.0673e-04 0 0 @@ -307,7 +307,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 3 + 3 2.0673e-04 0 0 @@ -327,7 +327,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 4 + 4 2.0673e-04 0 0 @@ -347,7 +347,7 @@ You can use the velocity controller and command linear velocity and yaw angular 1.269e-05 0.016754 command/motor_speed - 5 + 5 2.0673e-04 0 0 diff --git a/examples/worlds/nested_model_joint_positions.sdf b/examples/worlds/nested_model_joint_positions.sdf new file mode 100644 index 0000000000..273d205523 --- /dev/null +++ b/examples/worlds/nested_model_joint_positions.sdf @@ -0,0 +1,949 @@ + + + + + + + 0.001 + 0 + + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 0 0.5 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 1 1 0 + 1 1 0 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + rotor_joint + /model1/cmd_rotor + 30 + 0.05 + + + + + + 0 -2 0.5 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 1 1 0 + 1 1 0 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + 1 0 0 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 0 1 1 + 0 1 1 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + + base_link + model21::base_link + + + 1 + + + 0 + 0 + + 0 0 1 + + + + rotor_joint + /model2/cmd_rotor + 30 + 0.05 + + + model21::rotor_joint + /model21/cmd_rotor + 30 + 0.05 + + + + + + 0 -4 0.5 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 1 1 0 + 1 1 0 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + 1 0 0 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 0 1 1 + 0 1 1 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + + 2 0 0 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 1 0 1 + 1 0 1 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + + base_link + model31::base_link + + + 1 + + + 0 + 0 + + 0 0 1 + + + + model31::base_link + model32::base_link + + + 1 + + + 0 + 0 + + 0 0 1 + + + + rotor_joint + /model3/cmd_rotor + 30 + 0.05 + + + model31::rotor_joint + /model31/cmd_rotor + 30 + 0.05 + + + model32::rotor_joint + /model32/cmd_rotor + 30 + 0.05 + + + + + + 0 -6 0.5 0 0 0 + + 1 0 0 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 0 1 1 + 0 1 1 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + + 2 0 0 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 1 0 1 + 1 0 1 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + + model41::base_link + model42::base_link + + + 1 + + + 0 + 0 + + 0 0 1 + + + + model41::rotor_joint + /model41/cmd_rotor + 30 + 0.05 + + + model42::rotor_joint + /model42/cmd_rotor + 30 + 0.05 + + + + + diff --git a/examples/worlds/projector.sdf b/examples/worlds/projector.sdf new file mode 100644 index 0000000000..d2a601c348 --- /dev/null +++ b/examples/worlds/projector.sdf @@ -0,0 +1,274 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + true + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + -6 0 6 0 0.5 0 + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + IR Camera Left + floating + 350 + 315 + + IR_camera_left + false + + + + IR Camera Right + floating + 350 + 315 + 500 + + IR_camera_right + false + + + + RGB Camera + floating + 350 + 315 + 320 + + RGB_camera + false + + + + + + docked + + + + + + + docked + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 5 0.0 1.57 0 + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Camera with Projector + + + + + 0 0 0.5 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 1 1 1 + + + + + + + + 1 1 1 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + + + + + diff --git a/examples/worlds/quadcopter.sdf b/examples/worlds/quadcopter.sdf index 2cbda36f56..4b00c86c15 100644 --- a/examples/worlds/quadcopter.sdf +++ b/examples/worlds/quadcopter.sdf @@ -93,7 +93,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -113,7 +113,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -133,7 +133,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -153,7 +153,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index b1489b8476..9717bb11b3 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -152,7 +152,21 @@ - + + 1 + 30 + true + air_pressure + true + + + + 0.2 + 0.1 + + + + 1 100 diff --git a/include/gz/sim/Actor.hh b/include/gz/sim/Actor.hh new file mode 100644 index 0000000000..2cace92808 --- /dev/null +++ b/include/gz/sim/Actor.hh @@ -0,0 +1,161 @@ +/* + * 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. + * + */ +#ifndef GZ_SIM_ACTOR_HH_ +#define GZ_SIM_ACTOR_HH_ + +#include +#include +#include + +#include + +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Actor Actor.hh gz/sim/Actor.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a actor + /// entity. + /// + /// For example, given an actor's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Actor actor(entity); + /// std::string name = actor.Name(ecm); + /// + class GZ_SIM_VISIBLE Actor + { + /// \brief Constructor + /// \param[in] _entity Actor entity + public: explicit Actor(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Actor is related to. + /// \return Actor entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New actor entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this actor correctly refers to an entity that + /// has a components::Actor. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid actor in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the actor's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Actor's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the actor. + /// If the actor has a trajectory, this will only return the origin + /// pose of the trajectory and not the actual world pose of the actor. + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the actor or nullopt if the entity does not + /// have a components::Pose component. + /// \sa WorldPose + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the trajectory pose of the actor. There are two + /// ways that the actor can follow a trajectory: 1) SDF script, + /// 2) manually setting trajectory pose. This function retrieves 2) the + /// manual trajectory pose set by the user. The Trajectory pose is + /// given relative to the trajectory pose origin returned by Pose(). + /// \param[in] _ecm Entity Component manager. + /// \return Trajectory pose of the actor w.r.t. to trajectory origin. + /// \sa Pose + public: std::optional TrajectoryPose( + const EntityComponentManager &_ecm) const; + + /// \brief Set the trajectory pose of the actor. There are two + /// ways that the actor can follow a trajectory: 1) SDF script, + /// 2) manually setting trajectory pose. This function enables option 2). + /// Manually setting the trajectory pose will override the scripted + /// trajectory specified in SDF. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _name Trajectory pose w.r.t. to the trajectory origin + /// \sa Pose + public: void SetTrajectoryPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose); + + /// \brief Get the world pose of the actor. + /// This returns the current world pose of the actor computed by gazebo. + /// The world pose is the combination of the actor's pose and its + /// trajectory pose. The currently trajectory pose is either manually set + /// via SetTrajectoryPose or interpolated from waypoints in the SDF script + /// based on the current time. + /// \param[in] _ecm Entity-component manager. + /// \return World pose of the actor or nullopt if the entity does not + /// have a components::WorldPose component. + /// \sa Pose + public: std::optional WorldPose( + const EntityComponentManager &_ecm) const; + + /// \brief Set the name of animation to use for this actor. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _name Animation name + public: void SetAnimationName(EntityComponentManager &_ecm, + const std::string &_name); + + /// \brief Set the time of animation for this actor. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _time Animation time + public: void SetAnimationTime(EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_time); + + /// \brief Get the name of animation used by the actor + /// \param[in] _ecm Entity-component manager. + /// \return Animation name + public: std::optional AnimationName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the time of animation for this actor + /// \param[in] _ecm Entity-component manager. + /// \return Animation time + public: std::optional AnimationTime( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/Conversions.hh b/include/gz/sim/Conversions.hh index 4979f5650b..527c383409 100644 --- a/include/gz/sim/Conversions.hh +++ b/include/gz/sim/Conversions.hh @@ -54,6 +54,7 @@ #include #include #include +#include #include #include @@ -714,6 +715,43 @@ namespace gz template<> sdf::ParticleEmitter convert(const msgs::ParticleEmitter &_in); + /// \brief Generic conversion from a projector SDF object to another + /// type. + /// \param[in] _in Projector SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const sdf::Projector &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a projector SDF object to + /// a projector message object. + /// \param[in] _in Projector SDF object. + /// \return Projector message. + template<> + msgs::Projector convert(const sdf::Projector &_in); + + /// \brief Generic conversion from a projector SDF object to another + /// type. + /// \param[in] _in Projector SDF object. + /// \return Conversion result. + /// \tparam Out Output type. + template + Out convert(const msgs::Projector &/*_in*/) + { + Out::ConversionNotImplemented; + } + + /// \brief Specialized conversion from a projector SDF object to + /// a projector message object. + /// \param[in] _in Projecotr SDF object. + /// \return Projector message. + template<> + sdf::Projector convert(const msgs::Projector &_in); + + /// \brief Generic conversion from an SDF element to another type. /// \param[in] _in SDF element. /// \return Conversion result. diff --git a/include/gz/sim/EventManager.hh b/include/gz/sim/EventManager.hh index 02c97561d0..ace4848c04 100644 --- a/include/gz/sim/EventManager.hh +++ b/include/gz/sim/EventManager.hh @@ -118,6 +118,31 @@ namespace gz } } + /// \brief Get connection count for a particular event + /// Connection count for the event + public: template + unsigned int + ConnectionCount() + { + if (this->events.find(typeid(E)) == this->events.end()) + { + return 0u; + } + + E *eventPtr = dynamic_cast(this->events[typeid(E)].get()); + // All values in the map should be derived from Event, + // so this shouldn't be an issue, but it doesn't hurt to check. + if (eventPtr != nullptr) + { + return eventPtr->ConnectionCount(); + } + else + { + gzerr << "Failed to get connection count for event: " + << typeid(E).name() << std::endl; + return 0u; + } + } /// \brief Convenience type for storing typeinfo references. private: using TypeInfoRef = std::reference_wrapper; diff --git a/include/gz/sim/Joint.hh b/include/gz/sim/Joint.hh new file mode 100644 index 0000000000..8de460c544 --- /dev/null +++ b/include/gz/sim/Joint.hh @@ -0,0 +1,284 @@ +/* + * 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. + * + */ +#ifndef GZ_SIM_JOINT_HH_ +#define GZ_SIM_JOINT_HH_ + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Model.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Joint Joint.hh gz/sim/Joint.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a joint + /// entity. + /// + /// For example, given a joint's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Joint joint(entity); + /// std::string name = joint.Name(ecm); + /// + class GZ_SIM_VISIBLE Joint + { + /// \brief Constructor + /// \param[in] _entity Joint entity + public: explicit Joint(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Joint is related to. + /// \return Joint entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New joint entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this joint correctly refers to an entity that + /// has a components::Joint. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid joint in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the joint's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Joint's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent link name + /// \param[in] _ecm Entity-component manager. + /// \return Parent link name or nullopt if the entity does not have a + /// components::ParentLinkName component. + public: std::optional ParentLinkName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the child link name + /// \param[in] _ecm Entity-component manager. + /// \return Child link name or nullopt if the entity does not have a + /// components::ChildLinkName component. + public: std::optional ChildLinkName( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the joint or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the thread pitch of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Thread pitch of the joint or nullopt if the entity does not + /// have a components::ThreadPitch component. + public: std::optional ThreadPitch( + const EntityComponentManager &_ecm) const; + + /// \brief Get the joint axis + /// \param[in] _ecm Entity-component manager. + /// \return Axis of the joint or nullopt if the entity does not + /// have a components::JointAxis or components::JointAxis2 component. + public: std::optional> Axis( + const EntityComponentManager &_ecm) const; + + /// \brief Get the joint type + /// \param[in] _ecm Entity-component manager. + /// \return Type of the joint or nullopt if the entity does not + /// have a components::JointType component. + public: std::optional Type( + const EntityComponentManager &_ecm) const; + + /// \brief Get the ID of a sensor entity which is an immediate child of + /// this joint. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _name Sensor name. + /// \return Sensor entity. + public: sim::Entity SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const; + + /// \brief Get all sensors which are immediate children of this joint. + /// \param[in] _ecm Entity-component manager. + /// \return All sensors in this joint. + public: std::vector Sensors( + const EntityComponentManager &_ecm) const; + + /// \brief Get the number of sensors which are immediate children of this + /// joint. + /// \param[in] _ecm Entity-component manager. + /// \return Number of sensors in this joint. + public: uint64_t SensorCount(const EntityComponentManager &_ecm) const; + + /// \brief Set velocity on this joint. Only applied if no forces are set + /// \param[in] _ecm Entity Component manager. + /// \param[in] _velocities Joint velocities commands (target velocities). + /// This is different from ResetVelocity in that this does not modify the + /// internal state of the joint. Instead, the physics engine is expected + /// to compute the necessary joint torque for the commanded velocity and + /// apply it in the next simulation step. The vector of velocities should + /// have the same size as the degrees of freedom of the joint. + public: void SetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities); + + /// \brief Set force on this joint. If both forces and velocities are set, + /// only forces are applied + /// \param[in] _ecm Entity Component manager. + /// \param[in] _forces Joint force or torque commands (target forces + /// or toruqes). The vector of forces should have the same size as the + /// degrees of freedom of the joint. + public: void SetForce(EntityComponentManager &_ecm, + const std::vector &_forces); + + /// \brief Set the velocity limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum velocity limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetVelocityLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Set the effort limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum effort limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetEffortLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Set the position limits on a joint axis. + /// \param[in] _ecm Entity Component manager. + /// \param[in] _limits Joint limits to set to. The X() component of the + /// Vector2 specifies the minimum position limit, the Y() component stands + /// for maximum limit. The vector of limits should have the same size as + /// the degrees of freedom of the joint. + public: void SetPositionLimits(EntityComponentManager &_ecm, + const std::vector &_limits); + + /// \brief Reset the joint positions + /// \param[in] _ecm Entity Component manager. + /// \param[in] _positions Joint positions to reset to. + /// The vector of positions should have the same size as the degrees of + /// freedom of the joint. + public: void ResetPosition(EntityComponentManager &_ecm, + const std::vector &_positions); + + /// \brief Reset the joint velocities + /// \param[in] _ecm Entity Component manager. + /// \param[in] _velocities Joint velocities to reset to. This is different + /// from SetVelocity as this modifies the internal state of the joint. + /// The vector of velocities should have the same size as the degrees of + /// freedom of the joint. + public: void ResetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities); + + /// \brief By default, Gazebo will not report velocities for a joint, so + /// functions like `Velocity` will return nullopt. This + /// function can be used to enable joint velocity check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableVelocityCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief By default, Gazebo will not report positions for a joint, so + /// functions like `Position` will return nullopt. This + /// function can be used to enable joint position check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnablePositionCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief By default, Gazebo will not report transmitted wrench for a + /// joint, so functions like `TransmittedWrench` will return nullopt. This + /// function can be used to enable joint transmitted wrench check. + /// \param[in] _ecm Mutable reference to the ECM. + /// \param[in] _enable True to enable checks, false to disable. Defaults + /// to true. + public: void EnableTransmittedWrenchCheck(EntityComponentManager &_ecm, + bool _enable = true) const; + + /// \brief Get the velocity of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Velocity of the joint or nullopt if velocity check + /// is not enabled. + /// \sa EnableVelocityCheck + public: std::optional> Velocity( + const EntityComponentManager &_ecm) const; + + /// \brief Get the position of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Position of the joint or nullopt if position check + /// is not enabled. + /// \sa EnablePositionCheck + public: std::optional> Position( + const EntityComponentManager &_ecm) const; + + /// \brief Get the position of the joint + /// \param[in] _ecm Entity-component manager. + /// \return Transmitted wrench of the joint or nullopt if transmitted + /// wrench check is not enabled. + /// \sa EnableTransmittedWrenchCheck + public: std::optional> TransmittedWrench( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent model + /// \param[in] _ecm Entity-component manager. + /// \return Parent Model or nullopt if the entity does not have a + /// components::ParentEntity component. + public: std::optional ParentModel( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/Light.hh b/include/gz/sim/Light.hh new file mode 100644 index 0000000000..13cad9c3be --- /dev/null +++ b/include/gz/sim/Light.hh @@ -0,0 +1,291 @@ +/* + * 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. + * + */ +#ifndef GZ_SIM_LIGHT_HH_ +#define GZ_SIM_LIGHT_HH_ + +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Light Light.hh gz/sim/Light.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a light + /// entity. + /// + /// For example, given a light's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Light light(entity); + /// std::string name = light.Name(ecm); + /// + class GZ_SIM_VISIBLE Light + { + /// \brief Constructor + /// \param[in] _entity Light entity + public: explicit Light(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Light is related to. + /// \return Light entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New light entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this light correctly refers to an entity that + /// has a components::Light. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid light in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the light's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Light's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the light. + /// The pose is given w.r.t the light's parent. which can be a world or + /// a link. + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the light or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light type + /// \param[in] _ecm Entity-component manager. + /// \return Type of the light or nullopt if the entity does not + /// have a components::LightType component. + public: std::optional Type( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light diffuse color + /// \param[in] _ecm Entity-component manager. + /// \return Diffuse color of the light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional DiffuseColor( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light specular color + /// \param[in] _ecm Entity-component manager. + /// \return Specular color of the light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional SpecularColor( + const EntityComponentManager &_ecm) const; + + /// \brief Get whether the light casts shadows + /// \param[in] _ecm Entity-component manager. + /// \return Cast shadow bool value of light or nullopt if the entity does + /// not have a components::CastShadows component. + public: std::optional CastShadows( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light intensity. + /// \param[in] _ecm Entity-component manager. + /// \return Intensity of light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional Intensity( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light direction. + /// \param[in] _ecm Entity-component manager. + /// \return Direction of light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional Direction( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation range. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation range of light or nullopt if the entity does not + /// have a components::Light component. + public: std::optional AttenuationRange( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation constant value. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation constant value of light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional AttenuationConstant( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation linear value. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation linear value of light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional AttenuationLinear( + const EntityComponentManager &_ecm) const; + + /// \brief Get the light attenuation quadratic value. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _ecm Entity-component manager. + /// \return Attenuation quadratic value of light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional AttenuationQuadratic( + const EntityComponentManager &_ecm) const; + + /// \brief Get the inner angle of light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \return Inner angle of spot light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional SpotInnerAngle( + const EntityComponentManager &_ecm) const; + + /// \brief Get the outer angle of light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \return Outer angle of spot light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional SpotOuterAngle( + const EntityComponentManager &_ecm) const; + + /// \brief Get the fall off value of light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \return Fall off value of spot light or nullopt if the entity + /// does not have a components::Light component. + public: std::optional SpotFalloff( + const EntityComponentManager &_ecm) const; + + /// \brief Set the pose of this light. + /// \param[in] _ecm Entity-component manager. + /// Pose is set w.r.t. the light's parent which can be a world or a link. + /// \param[in] _pose Pose to set the light to. + public: void SetPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose); + + /// \brief Set the diffuse color of this light. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _color Diffuse color to set the light to + public: void SetDiffuseColor(EntityComponentManager &_ecm, + const math::Color &_color); + + /// \brief Set the specular color of this light. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _color Specular color to set the light to + public: void SetSpecularColor(EntityComponentManager &_ecm, + const math::Color &_color); + + /// \brief Set whether the light casts shadows. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _bool True to cast shadows, false to not cast shadows. + public: void SetCastShadows(EntityComponentManager &_ecm, + bool _castShadows); + + /// \brief Set light intensity. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _value Intensity value + public: void SetIntensity(EntityComponentManager &_ecm, + double _value); + + /// \brief Set light direction. Applies to directional lights + /// \param[in] _ecm Entity-component manager. + /// and spot lights only. + /// \param[in] _dir Direction to set + public: void SetDirection(EntityComponentManager &_ecm, + const math::Vector3d &_dir); + + /// \brief Set attenuation range of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _bool True to cast shadows, false to not cast shadows. + public: void SetAttenuationRange(EntityComponentManager &_ecm, + double _range); + + /// \brief Set attenuation constant value of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _value Attenuation constant value to set + public: void SetAttenuationConstant(EntityComponentManager &_ecm, + double _value); + + /// \brief Set attenuation linear value of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _value Attenuation linear value to set + public: void SetAttenuationLinear(EntityComponentManager &_ecm, + double _value); + + /// \brief Set attenuation quadratic value of this light. + /// \param[in] _ecm Entity-component manager. + /// Light attenuation is not applicable to directional lights. + /// \param[in] _value Attenuation quadratic value to set + public: void SetAttenuationQuadratic(EntityComponentManager &_ecm, + double _value); + + /// \brief Set inner angle for this light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _angle Angle to set. + public: void SetSpotInnerAngle(EntityComponentManager &_ecm, + const math::Angle &_angle); + + /// \brief Set outer angle for this light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _angle Angle to set. + public: void SetSpotOuterAngle(EntityComponentManager &_ecm, + const math::Angle &_angle); + + /// \brief Set fall off value for this light. Applies to spot lights only. + /// \param[in] _ecm Entity-component manager. + /// \param[in] _fallOff Fall off value + public: void SetSpotFalloff(EntityComponentManager &_ecm, + double _falloff); + + /// \brief Get the parent entity. This can be a world or a link. + /// \param[in] _ecm Entity-component manager. + /// \return Parent entity or nullopt if the entity does not have a + /// components::ParentEntity component. + public: std::optional Parent( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/SdfEntityCreator.hh b/include/gz/sim/SdfEntityCreator.hh index d65a20aae4..8e5292ef10 100644 --- a/include/gz/sim/SdfEntityCreator.hh +++ b/include/gz/sim/SdfEntityCreator.hh @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -155,6 +156,12 @@ namespace gz /// \sa CreateEntities(const sdf::Link *) public: Entity CreateEntities(const sdf::ParticleEmitter *_emitter); + /// \brief Create all entities that exist in the + /// sdf::Projector object. + /// \param[in] _projector SDF Projector object. + /// \return Projector entity. + public: Entity CreateEntities(const sdf::Projector *_projector); + /// \brief Request an entity deletion. This will insert the request /// into a queue. The queue is processed toward the end of a simulation /// update step. diff --git a/include/gz/sim/Sensor.hh b/include/gz/sim/Sensor.hh new file mode 100644 index 0000000000..e2558bee99 --- /dev/null +++ b/include/gz/sim/Sensor.hh @@ -0,0 +1,115 @@ +/* + * 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. + * + */ +#ifndef GZ_SIM_SENSOR_HH_ +#define GZ_SIM_SENSOR_HH_ + +#include +#include +#include + +#include + +#include + +#include + +#include "gz/sim/config.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Export.hh" +#include "gz/sim/Types.hh" + +namespace gz +{ + namespace sim + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SIM_VERSION_NAMESPACE { + // + /// \class Sensor Sensor.hh gz/sim/Sensor.hh + /// \brief This class provides wrappers around entities and components + /// which are more convenient and straight-forward to use than dealing + /// with the `EntityComponentManager` directly. + /// All the functions provided here are meant to be used with a sensor + /// entity. + /// + /// For example, given a sensor's entity, find the value of its + /// name component, one could use the entity-component manager (`ecm`) + /// directly as follows: + /// + /// std::string name = ecm.Component(entity)->Data(); + /// + /// Using this class however, the same information can be obtained with + /// a simpler function call: + /// + /// Sensor sensor(entity); + /// std::string name = sensor.Name(ecm); + /// + class GZ_SIM_VISIBLE Sensor + { + /// \brief Constructor + /// \param[in] _entity Sensor entity + public: explicit Sensor(sim::Entity _entity = kNullEntity); + + /// \brief Get the entity which this Sensor is related to. + /// \return Sensor entity. + public: sim::Entity Entity() const; + + /// \brief Reset Entity to a new one + /// \param[in] _newEntity New sensor entity. + public: void ResetEntity(sim::Entity _newEntity); + + /// \brief Check whether this sensor correctly refers to an entity that + /// has a components::Sensor. + /// \param[in] _ecm Entity-component manager. + /// \return True if it's a valid sensor in the manager. + public: bool Valid(const EntityComponentManager &_ecm) const; + + /// \brief Get the sensor's unscoped name. + /// \param[in] _ecm Entity-component manager. + /// \return Sensor's name or nullopt if the entity does not have a + /// components::Name component + public: std::optional Name( + const EntityComponentManager &_ecm) const; + + /// \brief Get the pose of the sensor + /// \param[in] _ecm Entity-component manager. + /// \return Pose of the sensor or nullopt if the entity does not + /// have a components::Pose component. + public: std::optional Pose( + const EntityComponentManager &_ecm) const; + + /// \brief Get the topic of the sensor + /// \param[in] _ecm Entity-component manager. + /// \return Topic of the sensor or nullopt if the entity does not + /// have a components::SensorTopic component. + public: std::optional Topic( + const EntityComponentManager &_ecm) const; + + /// \brief Get the parent entity. This can be a link or a joint. + /// \param[in] _ecm Entity-component manager. + /// \return Parent entity or nullopt if the entity does not have a + /// components::ParentEntity component. + public: std::optional Parent( + const EntityComponentManager &_ecm) const; + + /// \brief Private data pointer. + GZ_UTILS_IMPL_PTR(dataPtr) + }; + } + } +} +#endif diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index c441133a26..582134f9bf 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -246,6 +246,15 @@ namespace gz /// an UpdateRate has not been set. public: std::optional UpdateRate() const; + /// \brief Set the initial simulation time in seconds. + /// \param[in] _initialSimTime The desired initial simulation time in + /// seconds. + public: void SetInitialSimTime(const double &_initialSimTime) const; + + /// \brief Get the initial simulation time in seconds. + /// \return The initial simulation time in seconds. + public: double InitialSimTime() const; + /// \brief Get whether the server is using the level system /// \return True if the server is set to use the level system public: bool UseLevels() const; diff --git a/include/gz/sim/System.hh b/include/gz/sim/System.hh index 042b062730..c5ef4ef965 100644 --- a/include/gz/sim/System.hh +++ b/include/gz/sim/System.hh @@ -25,6 +25,8 @@ #include #include +#include + #include namespace gz @@ -100,6 +102,20 @@ namespace gz EventManager &_eventMgr) = 0; }; + /// \class ISystemConfigureParameters ISystem.hh ignition/gazebo/System.hh + /// \brief Interface for a system that declares parameters. + /// + /// ISystemConfigureParameters::ConfigureParameters is called after + /// ISystemConfigure::Configure. + class ISystemConfigureParameters { + /// \brief Configure the parameters of the system. + /// \param[in] _registry The parameter registry. + public: virtual void ConfigureParameters( + gz::transport::parameters::ParametersRegistry &_registry, + EntityComponentManager &_ecm) = 0; + }; + + class ISystemReset { public: virtual void Reset(const UpdateInfo &_info, EntityComponentManager &_ecm) = 0; diff --git a/include/gz/sim/Types.hh b/include/gz/sim/Types.hh index da38b3a8b9..bd62e86db6 100644 --- a/include/gz/sim/Types.hh +++ b/include/gz/sim/Types.hh @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ #ifndef GZ_SIM_TYPES_HH_ #define GZ_SIM_TYPES_HH_ @@ -41,8 +41,8 @@ namespace gz /// paused. std::chrono::steady_clock::duration simTime{0}; - /// \brief Total wall clock time elapsed. This increases even if - /// simulation is paused. + /// \brief Total wall clock time elapsed while simulation is running. This + /// will not increase while paused. std::chrono::steady_clock::duration realTime{0}; /// \brief Simulation time handled during a single update. diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 9bb151deb1..8e84199ddf 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -48,6 +48,13 @@ namespace gz math::Pose3d GZ_SIM_VISIBLE worldPose(const Entity &_entity, const EntityComponentManager &_ecm); + /// \brief Helper function to compute world velocity of an entity + /// \param[in] _entity Entity to get the world pose for + /// \param[in] _ecm Immutable reference to ECM. + /// \return World pose of entity + math::Vector3d GZ_SIM_VISIBLE relativeVel(const Entity &_entity, + const EntityComponentManager &_ecm); + /// \brief Helper function to generate scoped name for an entity. /// \param[in] _entity Entity to get the name for. /// \param[in] _ecm Immutable reference to ECM. diff --git a/include/gz/sim/components/AirSpeedSensor.hh b/include/gz/sim/components/AirSpeedSensor.hh new file mode 100644 index 0000000000..c4c9a52fbd --- /dev/null +++ b/include/gz/sim/components/AirSpeedSensor.hh @@ -0,0 +1,46 @@ +/* + * 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. + * +*/ +#ifndef GZ_SIM_COMPONENTS_AIRSPEED_HH_ +#define GZ_SIM_COMPONENTS_AIRSPEED_HH_ + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains an air speed sensor, + /// sdf::AirSpeed, information. + using AirSpeedSensor = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.AirSpeedSensor", + AirSpeedSensor) +} +} +} +} + +#endif diff --git a/include/gz/sim/components/BatteryPowerLoad.hh b/include/gz/sim/components/BatteryPowerLoad.hh new file mode 100644 index 0000000000..a4bb9d5abb --- /dev/null +++ b/include/gz/sim/components/BatteryPowerLoad.hh @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_SIM_COMPONENTS_BATTERYPOWERLOAD_HH_ +#define GZ_SIM_COMPONENTS_BATTERYPOWERLOAD_HH_ + +#include +#include +#include + +namespace gz::sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief Data structure to hold the consumer power load + /// and the name of the battery it uses. + struct BatteryPowerLoadInfo + { + /// \brief Entity of the battery to use. + Entity batteryId; + /// \brief Battery power load (W) to add to the battery. + double batteryPowerLoad; + }; + + /// \brief A component that indicates the total consumption of a battery. + using BatteryPowerLoad = + Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.BatteryPowerLoad", + BatteryPowerLoad) +} +} +} // namespace gz::sim + +#endif // GZ_SIM_COMPONENTS_BATTERYPOWERLOAD_HH_ diff --git a/include/gz/sim/components/Component.hh b/include/gz/sim/components/Component.hh index 48289178fa..24064b0eb4 100644 --- a/include/gz/sim/components/Component.hh +++ b/include/gz/sim/components/Component.hh @@ -387,6 +387,8 @@ namespace components /// \brief Unique name for this component type. This is set through the /// Factory registration. + // TODO(azeey) Change to const char* in Harmonic to prevent static + // initialization order fiasco. public: inline static std::string typeName; }; @@ -433,6 +435,8 @@ namespace components /// \brief Unique name for this component type. This is set through the /// Factory registration. + // TODO(azeey) Change to const char* in Harmonic to prevent static + // initialization order fiasco. public: inline static std::string typeName; }; diff --git a/include/gz/sim/components/Factory.hh b/include/gz/sim/components/Factory.hh index 5ea38c00b4..7031dd4154 100644 --- a/include/gz/sim/components/Factory.hh +++ b/include/gz/sim/components/Factory.hh @@ -19,9 +19,11 @@ #include #include +#include #include #include #include +#include #include #include @@ -77,26 +79,162 @@ namespace components } }; + /// \brief A wrapper around uintptr_t to prevent implicit conversions. + struct RegistrationObjectId + { + /// \brief Construct object from a pointer. + /// \param[in] _ptr Arbitrary pointer. + explicit RegistrationObjectId(void *_ptr) + : id(reinterpret_cast(_ptr)) + { + } + + /// \brief Construct object from a uintptr_t. + /// \param[in] _ptr Arbitrary pointer address. + explicit RegistrationObjectId(std::uintptr_t _ptrAddress) + : id(_ptrAddress) + { + } + + /// \brief Equality comparison. + /// \param[in] _other Other RegistrationObjectId object to compare against. + bool operator==(const RegistrationObjectId &_other) const + { + return this->id == _other.id; + } + + /// \brief Wrapped uintptr_t variable. + std::uintptr_t id; + }; + + + /// \brief A class to hold the queue of component descriptors registered by + /// translation units. This queue is necessary to ensure that component + /// creation continues to work after plugins are unloaded. The typical + /// scenario this aims to solve is: + /// 1. Plugin P1 registers component descripter for component C1. + /// 2. Plugin P1 gets unloaded. + /// 3. Plugin P2 registers a component descriptor for component C1 and tries + /// to create an instance of C1. + /// When P1 gets unloaded, the destructor of the static component + /// registration object calls Factory::Unregister which removes the component + /// descriptor from the queue. Without this step, P2 would attempt to use + /// the component descriptor created by P1 in step 3 and likely segfault + /// because the memory associated with that descriptor has been deleted when + /// P1 was unloaded. + class ComponentDescriptorQueue + { + /// \brief Check if the queue is empty + public: bool GZ_SIM_HIDDEN Empty() + { + return this->queue.empty(); + } + + /// \brief Add a component descriptor to the queue + /// \param[in] _regObjId An ID that identifies the registration object. This + /// is generally derived from the `this` pointer of the static component + /// registration object created when calling GZ_SIM_REGISTER_COMPONENT. + /// \param[in] _comp The component descriptor + public: void GZ_SIM_HIDDEN Add(RegistrationObjectId _regObjId, + ComponentDescriptorBase *_comp) + { + this->queue.push_front({_regObjId, _comp}); + } + + /// \brief Remove a component descriptor from the queue. This also deletes + /// memory allocated for the component descriptor by the static component + /// registration object. + /// \param[in] _regObjId An ID that identifies the registration object. This + /// is generally derived from the `this` pointer of the static component + /// registration object created when calling GZ_SIM_REGISTER_COMPONENT. + public: void GZ_SIM_HIDDEN Remove(RegistrationObjectId _regObjId) + { + auto compIt = std::find_if(this->queue.rbegin(), this->queue.rend(), + [&](const auto &_item) + { return _item.first == _regObjId; }); + + if (compIt != this->queue.rend()) + { + ComponentDescriptorBase *compDesc = compIt->second; + this->queue.erase(std::prev(compIt.base())); + delete compDesc; + } + } + + /// \brief Create a component using the latest available component + /// descriptor. This simply forward to ComponentDescriptorBase::Create + /// \sa ComponentDescriptorBase::Create + public: GZ_SIM_HIDDEN std::unique_ptr Create() const + { + if (!this->queue.empty()) + { + return this->queue.front().second->Create(); + } + return {}; + } + + /// \brief Create a component using the latest available component + /// descriptor. This simply forward to ComponentDescriptorBase::Create + /// \sa ComponentDescriptorBase::Create + public: GZ_SIM_HIDDEN std::unique_ptr Create( + const components::BaseComponent *_data) const + { + if (!this->queue.empty()) + { + return this->queue.front().second->Create(_data); + } + return {}; + } + + /// \brief Queue of component descriptors registered by static registration + /// objects. + private: std::deque> queue; + }; + /// \brief A factory that generates a component based on a string type. + // TODO(azeey) Do not inherit from common::SingletonT in Harmonic class Factory : public gz::common::SingletonT { + // Deleted copy constructors to make the ABI checker happy + public: Factory(Factory &) = delete; + public: Factory(const Factory &) = delete; + // Since the copy constructors are deleted, we need to explicitly declare a + // default constructor. + // TODO(azeey) Make this private in Harmonic + public: Factory() = default; + + /// \brief Get an instance of the singleton + public: GZ_SIM_VISIBLE static Factory *Instance(); + /// \brief Register a component so that the factory can create instances /// of the component based on an ID. /// \param[in] _type Type of component to register. /// \param[in] _compDesc Object to manage the creation of ComponentTypeT /// objects. /// \tparam ComponentTypeT Type of component to register. - public: template + // TODO(azeey) Deprecate in favor of overload that takes _regObjId + public: template void Register(const std::string &_type, ComponentDescriptorBase *_compDesc) { - // Every time a plugin which uses a component type is loaded, it attempts - // to register it again, so we skip it. - if (ComponentTypeT::typeId != 0) - { - return; - } + this->Register(_type, _compDesc, + RegistrationObjectId{nullptr}); + } + /// \brief Register a component so that the factory can create instances + /// of the component based on an ID. + /// \param[in] _type Type of component to register. + /// \param[in] _compDesc Object to manage the creation of ComponentTypeT + /// objects. + /// \param[in] _regObjId An ID that identifies the registration object. This + /// is generally derived from the `this` pointer of the static component + /// registration object created when calling GZ_SIM_REGISTER_COMPONENT. + /// \tparam ComponentTypeT Type of component to register. + public: template + void Register(const std::string &_type, ComponentDescriptorBase *_compDesc, + RegistrationObjectId _regObjId) + { auto typeHash = gz::common::hash64(_type); // Initialize static member variable - we need to set these @@ -120,8 +258,8 @@ namespace components << runtimeNameIt->second << "] and type [" << runtimeName << "] with name [" << _type << "]. Second type will not work." << std::endl; + return; } - return; } // This happens at static initialization time, so we can't use common @@ -147,7 +285,7 @@ namespace components } // Keep track of all types - this->compsById[ComponentTypeT::typeId] = _compDesc; + this->compsById[ComponentTypeT::typeId].Add(_regObjId, _compDesc); namesById[ComponentTypeT::typeId] = ComponentTypeT::typeName; runtimeNamesById[ComponentTypeT::typeId] = runtimeName; } @@ -155,12 +293,23 @@ namespace components /// \brief Unregister a component so that the factory can't create instances /// of the component anymore. /// \tparam ComponentTypeT Type of component to unregister. - public: template + // TODO(azeey) Deprecate in favor of overload that takes _regObjId + public: template void Unregister() { - this->Unregister(ComponentTypeT::typeId); + this->Unregister(RegistrationObjectId{nullptr}); + } - ComponentTypeT::typeId = 0; + /// \brief Unregister a component so that the factory can't create instances + /// of the component anymore. + /// \tparam ComponentTypeT Type of component to unregister. + /// \param[in] _regObjId An ID that identifies the registration object. This + /// is generally derived from the `this` pointer of the static component + /// registration object created when calling GZ_SIM_REGISTER_COMPONENT. + public: template + void Unregister(RegistrationObjectId _regObjId) + { + this->Unregister(ComponentTypeT::typeId, _regObjId); } /// \brief Unregister a component so that the factory can't create instances @@ -169,36 +318,32 @@ namespace components /// within the component type itself. Prefer using the templated /// `Unregister` function when possible. /// \param[in] _typeId Type of component to unregister. + // TODO(azeey) Deprecate in favor of overload that takes _regObjId public: void Unregister(ComponentTypeId _typeId) { - // Not registered - if (_typeId == 0) - { - return; - } - - { - auto it = this->compsById.find(_typeId); - if (it != this->compsById.end()) - { - delete it->second; - this->compsById.erase(it); - } - } + this->Unregister(_typeId, RegistrationObjectId{nullptr}); + } + /// \brief Unregister a component so that the factory can't create instances + /// of the component anymore. + /// \details This function will not reset the `typeId` static variable + /// within the component type itself. Prefer using the templated + /// `Unregister` function when possible. + /// \param[in] _typeId Type of component to unregister. + /// \param[in] _regObjId An ID that identifies the registration object. This + /// is generally derived from the `this` pointer of the static component + /// registration object created when calling GZ_SIM_REGISTER_COMPONENT. + public: void Unregister(ComponentTypeId _typeId, + RegistrationObjectId _regObjId) + { + auto it = this->compsById.find(_typeId); + if (it != this->compsById.end()) { - auto it = namesById.find(_typeId); - if (it != namesById.end()) - { - namesById.erase(it); - } - } + it->second.Remove(_regObjId); - { - auto it = runtimeNamesById.find(_typeId); - if (it != runtimeNamesById.end()) + if (it->second.Empty()) { - runtimeNamesById.erase(it); + this->compsById.erase(it); } } } @@ -224,9 +369,10 @@ namespace components // Create a new component if a FactoryFn has been assigned to this type. std::unique_ptr comp; auto it = this->compsById.find(_type); - if (it != this->compsById.end() && nullptr != it->second) - comp = it->second->Create(); - + if (it != this->compsById.end()) + { + comp = it->second.Create(); + } return comp; } @@ -254,8 +400,10 @@ namespace components else { auto it = this->compsById.find(_type); - if (it != this->compsById.end() && nullptr != it->second) - comp = it->second->Create(_data); + if (it != this->compsById.end()) + { + comp = it->second.Create(_data); + } } return comp; @@ -292,19 +440,7 @@ namespace components } /// \brief A list of registered components where the key is its id. - /// - /// Note about compsByName and compsById. The maps store pointers as the - /// values, but never cleans them up, which may (at first glance) seem like - /// incorrect behavior. This is not a mistake. Since ComponentDescriptors - /// are created at the point in the code where components are defined, this - /// generally ends up in a shared library that will be loaded at runtime. - /// - /// Because this and the plugin loader both use static variables, and the - /// order of static initialization and destruction are not guaranteed, this - /// can lead to a scenario where the shared library is unloaded (with the - /// ComponentDescriptor), but the Factory still exists. For this reason, - /// we just keep a pointer, which will dangle until the program is shutdown. - private: std::map compsById; + private: std::map compsById; /// \brief A list of IDs and their equivalent names. public: std::map namesById; @@ -328,12 +464,20 @@ namespace components { \ public: GzSimComponents##_classname() \ { \ - if (_classname::typeId != 0) \ - return; \ using namespace gz;\ using Desc = sim::components::ComponentDescriptor<_classname>; \ sim::components::Factory::Instance()->Register<_classname>(\ - _compType, new Desc());\ + _compType, new Desc(), sim::components::RegistrationObjectId(this));\ + } \ + public: GzSimComponents##_classname( \ + const GzSimComponents##_classname&) = delete; \ + public: GzSimComponents##_classname( \ + GzSimComponents##_classname&) = delete; \ + public: ~GzSimComponents##_classname() \ + { \ + using namespace gz; \ + sim::components::Factory::Instance()->Unregister<_classname>( \ + sim::components::RegistrationObjectId(this)); \ } \ }; \ static GzSimComponents##_classname\ diff --git a/include/gz/sim/components/Projector.hh b/include/gz/sim/components/Projector.hh new file mode 100644 index 0000000000..9824e88594 --- /dev/null +++ b/include/gz/sim/components/Projector.hh @@ -0,0 +1,53 @@ +/* + * 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. + * +*/ +#ifndef GZ_SIM_COMPONENTS_PROJECTOR_HH_ +#define GZ_SIM_COMPONENTS_PROJECTOR_HH_ + +#include + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace serializers +{ + using ProjectorSerializer = + serializers::ComponentToMsgSerializer; +} + +namespace components +{ + /// \brief A component type that contains a projector, + /// sdf::Projector, information. + using Projector = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.Projector", Projector) +} +} +} +} + +#endif diff --git a/include/gz/sim/detail/EntityComponentManager.hh b/include/gz/sim/detail/EntityComponentManager.hh index 601458bf6f..7ce3096e4a 100644 --- a/include/gz/sim/detail/EntityComponentManager.hh +++ b/include/gz/sim/detail/EntityComponentManager.hh @@ -52,14 +52,18 @@ namespace traits template struct HasEqualityOperator { +#if !defined(_MSC_VER) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wnonnull" +#endif enum { // False positive codecheck "Using C-style cast" value = !std::is_same::value // NOLINT }; +#if !defined(_MSC_VER) #pragma GCC diagnostic pop +#endif }; } diff --git a/include/gz/sim/rendering/MarkerManager.hh b/include/gz/sim/rendering/MarkerManager.hh index 5be2a04cd9..3bed374c0d 100644 --- a/include/gz/sim/rendering/MarkerManager.hh +++ b/include/gz/sim/rendering/MarkerManager.hh @@ -66,6 +66,11 @@ class GZ_SIM_RENDERING_VISIBLE MarkerManager /// \param[in] _name Name of service public: void SetTopic(const std::string &_name); + /// \brief Clear the marker manager + /// Clears internal resources stored in the marker manager. + /// Note: this does not actually destroy the objects. + public: void Clear(); + /// \internal /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/include/gz/sim/rendering/RenderUtil.hh b/include/gz/sim/rendering/RenderUtil.hh index 17f99dc384..f03d609eee 100644 --- a/include/gz/sim/rendering/RenderUtil.hh +++ b/include/gz/sim/rendering/RenderUtil.hh @@ -53,6 +53,9 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// \brief Initialize the renderer. Must be called in the rendering thread. public: void Init(); + /// \brief Destroy the renderer. Must be called in the rendering thread. + public: void Destroy(); + /// \brief Count of pending sensors. Must be called in the rendering thread. /// \return Number of sensors to be added on the next `Update` call /// diff --git a/include/gz/sim/rendering/SceneManager.hh b/include/gz/sim/rendering/SceneManager.hh index 4d09ed7f86..2e7287075c 100644 --- a/include/gz/sim/rendering/SceneManager.hh +++ b/include/gz/sim/rendering/SceneManager.hh @@ -269,6 +269,14 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { public: rendering::ParticleEmitterPtr UpdateParticleEmitter(Entity _id, const msgs::ParticleEmitter &_emitter); + /// \brief Create a projector + /// \param[in] _id Unique projector id + /// \param[in] _projector Projector sdf dom + /// \param[in] _parentId Parent id + /// \return Projector object created from the sdf dom + public: rendering::ProjectorPtr CreateProjector( + Entity _id, const sdf::Projector &_projector, Entity _parentId); + /// \brief Gazebo Sensors is the one responsible for adding sensors /// to the scene. Here we just keep track of it and make sure it has /// the correct parent. @@ -373,6 +381,11 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { /// IDs are available public: Entity UniqueId() const; + /// \brief Clear the scene manager + /// Clears internal resources stored in the scene manager. + /// Note: this does not actually destroy the objects. + public: void Clear(); + /// \internal /// \brief Pointer to private data class private: std::unique_ptr dataPtr; diff --git a/src/Actor.cc b/src/Actor.cc new file mode 100644 index 0000000000..a949ac00b6 --- /dev/null +++ b/src/Actor.cc @@ -0,0 +1,169 @@ +/* + * 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 "gz/sim/components/Actor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" + +#include "gz/sim/Actor.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + +class gz::sim::Actor::Implementation +{ + /// \brief Id of actor entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Actor::Actor(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Actor::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Actor::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Actor::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Actor::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Actor::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Actor::WorldPose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +void Actor::SetTrajectoryPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose) +{ + auto pose = + _ecm.Component(this->dataPtr->id); + + if (!pose) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::TrajectoryPose(_pose)); + } + else + { + pose->Data() = _pose; + } +} + +////////////////////////////////////////////////// +std::optional Actor::TrajectoryPose( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +void Actor::SetAnimationName(EntityComponentManager &_ecm, + const std::string &_name) +{ + auto animName = + _ecm.Component(this->dataPtr->id); + + if (!animName) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::AnimationName(_name)); + } + else + { + animName->Data() = _name; + } +} + +////////////////////////////////////////////////// +void Actor::SetAnimationTime(EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_time) +{ + auto animTime = + _ecm.Component(this->dataPtr->id); + + if (!animTime) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::AnimationTime(_time)); + } + else + { + animTime->Data() = _time; + } +} + +////////////////////////////////////////////////// +std::optional Actor::AnimationName( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Actor::AnimationTime( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} diff --git a/src/Actor_TEST.cc b/src/Actor_TEST.cc new file mode 100644 index 0000000000..92f50e26df --- /dev/null +++ b/src/Actor_TEST.cc @@ -0,0 +1,82 @@ +/* + * 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 "gz/sim/Actor.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/components/Actor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" + +///////////////////////////////////////////////// +TEST(ActorTest, Constructor) +{ + gz::sim::Actor actorNull; + EXPECT_EQ(gz::sim::kNullEntity, actorNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + EXPECT_EQ(id, actor.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Actor actorCopy(actor); // NOLINT + EXPECT_EQ(actor.Entity(), actorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + gz::sim::Actor actorCopy; + actorCopy = actor; + EXPECT_EQ(actor.Entity(), actorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + gz::sim::Actor actorMoved(std::move(actor)); + EXPECT_EQ(id, actorMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(ActorTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Actor actor(id); + + gz::sim::Actor actorMoved; + actorMoved = std::move(actor); + EXPECT_EQ(id, actorMoved.Entity()); +} diff --git a/src/AddedMass_TEST.cc b/src/AddedMass_TEST.cc new file mode 100644 index 0000000000..5d7a74ca4d --- /dev/null +++ b/src/AddedMass_TEST.cc @@ -0,0 +1,673 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "../test/helpers/EnvTestFixture.hh" + +const char *kWorldFilePath{"/test/worlds/added_mass_full_matrix.sdf"}; +const char *kWorldName{"added_mass"}; +const char *kModelName{"body"}; +const char *kLinkName{"link"}; + +// Struct to define test inputs. +struct TestInputs { + gz::math::Vector3d pos; + gz::math::Quaternion quat; + gz::math::Vector3d lin_vel; + gz::math::Vector3d ang_vel; + gz::math::Vector3d force; + gz::math::Vector3d torque; +}; + +// Struct to define test outputs. +struct TestOutputs { + gz::math::Vector3d lin_acc; + gz::math::Vector3d ang_acc; +}; + +// Struct to define test cases (inputs and outputs). +struct TestCase { + TestInputs inputs; + TestOutputs outputs; +}; + +// Test case #1 +const TestCase kTestCase1{ + // Inputs + TestInputs{ + // pos + gz::math::Vector3d( + -0.2425045525045464, + 0.33840815652747214, + -0.15400580887792237 + ), + // quat + gz::math::Quaternion( + -0.2809847940832897, + -0.0670318259306688, + -0.6456490396607173, + 0.7068886739731832 + ), + // lin_vel + gz::math::Vector3d( + -0.39036334852467514, + -0.6319184973934608, + 0.5801096725227644 + ), + // ang_vel + gz::math::Vector3d( + 0.9586223737234041, + 0.3819110744112888, + 0.6723060779392143 + ), + // force + gz::math::Vector3d( + -0.556637080115808, + 0.05794391212068817, + -0.024721551602638447 + ), + // torque + gz::math::Vector3d( + -0.751375873274468, + 0.02394774300273772, + 0.8181098350410643 + ) + }, + // outputs + TestOutputs{ + // lin_acc + gz::math::Vector3d( + 0.1192543571637511, + 1.2564925342515267, + -1.5492015338978795 + ), + // ang_acc + gz::math::Vector3d( + 1.7232932412616608, + -1.1261918994329594, + -0.17792714753780803 + ) + } +}; + +// Test case #2 +const TestCase kTestCase2{ + // Inputs + TestInputs{ + // pos + gz::math::Vector3d( + -0.26627756134916636, + -0.7084085992300324, + 0.10619830845744738 + ), + // quat + gz::math::Quaternion( + -0.3052948472230469, + 0.9269624883895875, + 0.2095263494451089, + -0.0602852408624731 + ), + // lin_vel + gz::math::Vector3d( + -0.7458530424796268, + -0.1666649256184578, + 0.5386761683694596 + ), + // ang_vel + gz::math::Vector3d( + 0.17926634408173947, + -0.12034245997240012, + 0.18516904536562961 + ), + // force + gz::math::Vector3d( + -0.15924606852428447, + -0.9253978615421046, + -0.9435058673509571 + ), + // torque + gz::math::Vector3d( + -0.13533482138078679, + 0.03828213442605044, + -0.350293463974384 + ) + }, + // outputs + TestOutputs{ + // lin_acc + gz::math::Vector3d( + 0.42447316327242957, + -0.7080776570562612, + -1.2515188145460858 + ), + // ang_acc + gz::math::Vector3d( + 0.3114632455376882, + -1.4883476738089614, + 0.9047781278329061 + ) + } +}; + +// Test case #3 +const TestCase kTestCase3{ + // Inputs + TestInputs{ + // pos + gz::math::Vector3d( + 0.09486194918678748, + -0.9099264683313666, + 0.39781798711093663 + ), + // quat + gz::math::Quaternion( + 0.4474845382742349, + -0.49632065154724486, + 0.5178323029314619, + -0.5341096375220279 + ), + // lin_vel + gz::math::Vector3d( + 0.4774732593432809, + 0.9660133435982379, + -0.7444575664048005 + ), + // ang_vel + gz::math::Vector3d( + -0.6060956578366126, + 0.2115034622260703, + 0.9224794841428035 + ), + // force + gz::math::Vector3d( + 0.3454369446897425, + 0.6102403086661006, + 0.15097984754808258 + ), + // torque + gz::math::Vector3d( + 0.44071925082422103, + -0.6126203759247251, + 0.9009309436931738 + ) + }, + // outputs + TestOutputs{ + // lin_acc + gz::math::Vector3d( + 0.24671175337681506, + 2.20705594933898, + 2.7611862436175336 + ), + // ang_acc + gz::math::Vector3d( + 3.253196299221303, + -2.1915486730787777, + 2.4857656486324062 + ) + } +}; + +// Test case #4 +const TestCase kTestCase4{ + // Inputs + TestInputs{ + // pos + gz::math::Vector3d( + -0.021935430219797603, + -0.2028619676525827, + -0.7409964940861165 + ), + // quat + gz::math::Quaternion( + -0.21651876198721565, + -0.8484495285682753, + 0.16290189513073497, + 0.4546603080791308 + ), + // lin_vel + gz::math::Vector3d( + 0.0422249657383702, + -0.018269069242955682, + -0.08889923134418964 + ), + // ang_vel + gz::math::Vector3d( + 0.5054783278488537, + 0.9409816563774551, + -0.08228410518648444 + ), + // force + gz::math::Vector3d( + 0.5313699373453282, + 0.07366554324213204, + -0.11253372373538961 + ), + // torque + gz::math::Vector3d( + 0.33872369018889503, + 0.6522277135523256, + -0.5097460548473949 + ) + }, + // outputs + TestOutputs{ + // lin_acc + gz::math::Vector3d( + 0.23502926958161108, + 1.8946101802534714, + -1.6216718048541643 + ), + // ang_acc + gz::math::Vector3d( + -4.67488778909784, + 3.023629102947669, + -4.88653326608196 + ) + } +}; + +// Test case #5 +const TestCase kTestCase5{ + // Inputs + TestInputs{ + // pos + gz::math::Vector3d( + 0.9689583056406572, + 0.0017441433682259255, + 0.3493388951936456 + ), + // quat + gz::math::Quaternion( + -0.6634858326989433, + -0.46031657032694107, + 0.4932102494131894, + 0.3234792957269624 + ), + // lin_vel + gz::math::Vector3d( + 0.5964142494679903, + -0.3148625519885535, + 0.6209278519251678 + ), + // ang_vel + gz::math::Vector3d( + -0.43590121461970166, + 0.9146114641552883, + -0.5598223768473267 + ), + // force + gz::math::Vector3d( + -0.2998167392689237, + -0.2695157563478241, + 0.3651342901444177 + ), + // torque + gz::math::Vector3d( + -0.7188323924902038, + -0.7464458674658769, + -0.029814015736676858 + ) + }, + // outputs + TestOutputs{ + // lin_acc + gz::math::Vector3d( + 0.90004165730203, + 0.7398170763472348, + 0.3374987284916062 + ), + // ang_acc + gz::math::Vector3d( + -0.14893549985333834, + -1.0660445662071873, + -0.45274284576200496 + ) + } +}; + +// Vector of test cases. +const std::vector kTestCases{ + kTestCase1, + kTestCase2, + kTestCase3, + kTestCase4, + kTestCase5 +}; + +// Number of test cases. +const std::size_t kNumCases{kTestCases.size()}; + + +// Set the body's pose, velocity, and wrench, in PreUpdate; check reported +// accelerations in PostUpdate. +class AccelerationCheckPlugin: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemReset, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate +{ + public: AccelerationCheckPlugin() = default; + + public: void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr + ) override; + + public: void Reset( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm + ) override; + + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm + ) override; + + public: void PostUpdate( + const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm + ) override; + + // Whether to perform a check against the expected output values in + // PostUpdate. + public: bool performChecks{false}; + + // Determines which test case to use when setting up input values in + // PreUpdate, and when checking output values in PostUpdate. + public: int caseNumber{0}; + + // Used to ensure that all the tests have run. + public: int testCounter{0}; + + private: void InitializeModelAndLink(gz::sim::EntityComponentManager &_ecm); + + private: gz::sim::Entity model_entity{gz::sim::kNullEntity}; + + private: gz::sim::Entity link_entity{gz::sim::kNullEntity}; +}; + +// Used by Configure and Reset to initialize the body and link ECM state. +void AccelerationCheckPlugin::InitializeModelAndLink( + gz::sim::EntityComponentManager &_ecm + ) + { + gzdbg << "Initializing model." << std::endl; + if (this->model_entity == gz::sim::kNullEntity) { + this-> model_entity = _ecm.EntityByComponents( + gz::sim::components::Name(kModelName), + gz::sim::components::Model() + ); + ASSERT_NE(this->model_entity, gz::sim::kNullEntity); + } + gz::sim::Model model = gz::sim::Model(this->model_entity); + ASSERT_TRUE(model.Valid(_ecm)); + ASSERT_NE( + _ecm.CreateComponent(this->model_entity, gz::sim::components::WorldPose()), + nullptr + ); + + gzdbg << "Initializing link." << std::endl; + if (this->link_entity == gz::sim::kNullEntity) { + this->link_entity = model.LinkByName(_ecm, kLinkName); + ASSERT_NE(this->link_entity, gz::sim::kNullEntity); + } + gz::sim::Link link = gz::sim::Link(this->link_entity); + ASSERT_TRUE(link.Valid(_ecm)); + ASSERT_NE( + _ecm.CreateComponent( + this->link_entity, + gz::sim::components::WorldLinearVelocity() + ), + nullptr + ); + ASSERT_NE( + _ecm.CreateComponent( + this->link_entity, + gz::sim::components::WorldAngularVelocity() + ), + nullptr + ); + ASSERT_NE( + _ecm.CreateComponent( + this->link_entity, + gz::sim::components::WorldLinearAcceleration() + ), + nullptr + ); + ASSERT_NE( + _ecm.CreateComponent( + this->link_entity, + gz::sim::components::WorldAngularAcceleration() + ), + nullptr + ); +} + +// Sets model and link ECM state. +void AccelerationCheckPlugin::Configure( + const gz::sim::Entity &, + const std::shared_ptr &, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager & +) +{ + gzdbg << "Configure happening." << std::endl; + this->InitializeModelAndLink(_ecm); +} + +// Sets model and link ECM state. +void AccelerationCheckPlugin::Reset( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &_ecm +) +{ + gzdbg << "Reset happening." << std::endl; + this->InitializeModelAndLink(_ecm); +} + +// Set pose, velocity, and wrench (force and torque). +void AccelerationCheckPlugin::PreUpdate( + const gz::sim::UpdateInfo &, + gz::sim::EntityComponentManager &_ecm +) +{ + gzdbg << "PreUpdate happening." << std::endl; + + ASSERT_NE(this->model_entity, gz::sim::kNullEntity); + gz::sim::Model model = gz::sim::Model(this->model_entity); + ASSERT_TRUE(model.Valid(_ecm)); + ASSERT_NE(this->link_entity, gz::sim::kNullEntity); + gz::sim::Link link = gz::sim::Link(this->link_entity); + ASSERT_TRUE(link.Valid(_ecm)); + + const TestInputs inputs = kTestCases[this->caseNumber].inputs; + + const gz::math::Vector3d pos = inputs.pos; + const gz::math::Quaternion quat = inputs.quat; + const gz::math::Pose3d world_pose = gz::math::Pose3d(pos, quat); + gzdbg << "Setting model world position to:\t" << pos << std::endl; + gzdbg << "Setting model world orientation to:\t" << quat << std::endl; + _ecm.SetComponentData( + this->model_entity, + world_pose + ); + + const gz::math::Vector3d lin_vel = inputs.lin_vel; + gzdbg << "Setting link world linear velocity to:\t" << lin_vel << std::endl; + // `LinearVelocityCmd` sets linear velocity in the link's frame, and is + // unaffected by the rotation performed by `WorldPoseCmd`. + _ecm.SetComponentData( + this->link_entity, + lin_vel + ); + const gz::math::Vector3d ang_vel = inputs.ang_vel; + gzdbg << "Setting link world angular velocity to:\t" << ang_vel << std::endl; + // `AngularVelocityCmd` sets angular velocity in the link's frame, and is + // unaffected by the rotation performed by `WorldPoseCmd`. + _ecm.SetComponentData( + this->link_entity, + ang_vel + ); + + const gz::math::Vector3d force = inputs.force; + const gz::math::Vector3d torque = inputs.torque; + gzdbg << "Setting link world force to: \t" << force << std::endl; + gzdbg << "Setting link world torque to:\t" << torque << std::endl; + // The entity component manager first registers the wrench with the link and + // then applies the the rotation from `WorldPoseCmd`. This results in a world + // wrench that is affected by the rotation in `WorldPoseCmd`, we need to + // correct for this here. + link.AddWorldWrench(_ecm, quat.Inverse() * force, quat.Inverse() * torque); +} + +// Check linear and angular acceleration. +void AccelerationCheckPlugin::PostUpdate( + const gz::sim::UpdateInfo &, + const gz::sim::EntityComponentManager &_ecm +) +{ + gzdbg << "PostUpdate happening." << std::endl; + + if (this->performChecks) { + gzdbg << "Performing checks." << std::endl; + this->testCounter += 1; + gzdbg << "Check number " << this->testCounter << std::endl; + + ASSERT_NE(this->link_entity, gz::sim::kNullEntity); + gz::sim::Link link = gz::sim::Link(this->link_entity); + ASSERT_TRUE(link.Valid(_ecm)); + TestOutputs outputs = kTestCases[this->caseNumber].outputs; + + std::optional maybe_lin_acc = + link.WorldLinearAcceleration(_ecm); + EXPECT_TRUE(maybe_lin_acc); + gzdbg << "Expected world linear acceleration:\t" << outputs.lin_acc << + std::endl; + if (maybe_lin_acc) { + gz::math::Vector3d lin_acc = maybe_lin_acc.value(); + gzdbg << "Actual world linear acceleration:\t" << lin_acc << std::endl; + EXPECT_LT((lin_acc - outputs.lin_acc).Length(), 1e-2); + } + else + { + gzdbg << "Unable to retrieve link world linear acceleration." << + std::endl; + } + + std::optional maybe_ang_acc = + link.WorldAngularAcceleration(_ecm); + EXPECT_TRUE(maybe_ang_acc); + gzdbg << "Expected world angular acceleration:\t" << outputs.ang_acc << + std::endl; + if (maybe_ang_acc) { + gz::math::Vector3d ang_acc = maybe_ang_acc.value(); + gzdbg << "Actual world angular acceleration:\t" << ang_acc << + std::endl; + EXPECT_LT((ang_acc - outputs.ang_acc).Length(), 1e-2); + } + else + { + gzdbg << "Unable to retrieve link world angular acceleration." << + std::endl; + } + } +} + +// Request a world reset via transport. +void requestWorldReset() +{ + std::string topic{"/world/" + std::string(kWorldName) + "/control"}; + gz::msgs::WorldControl req; + gz::msgs::Boolean rep; + req.mutable_reset()->set_all(true); + gz::transport::Node node; + + gzdbg << "Requesting world reset." << std::endl; + unsigned int timeout = 1000; + bool result; + bool executed = + node.Request(topic, req, timeout, rep, result); + ASSERT_TRUE(executed); + ASSERT_TRUE(result); + ASSERT_TRUE(rep.data()); +} + +class EmptyTestFixture: public InternalFixture<::testing::Test> {}; + +// Check that the accelerations reported for a body with added mass matche the +// expected values. +TEST_F(EmptyTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(AddedMassAccelerationTest)) +{ + gz::sim::ServerConfig serverConfig; + serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, kWorldFilePath) + ); + gz::sim::Server server = gz::sim::Server(serverConfig); + std::shared_ptr accelerationChecker = + std::make_shared(); + std::optional maybe_system_added = + server.AddSystem(accelerationChecker); + ASSERT_TRUE(maybe_system_added); + if (maybe_system_added) { + ASSERT_TRUE(maybe_system_added.value()); + } + ASSERT_FALSE(server.Running()); + + // Run one iteration in order to "initialize" physics. + ASSERT_TRUE(server.RunOnce(false)); + + // For each test case, reset the server and run checks. + for (std::size_t i = 0; i < kNumCases; i++) { + accelerationChecker->performChecks = false; + accelerationChecker->caseNumber = i; + requestWorldReset(); + // It takes two iterations for a reset to happen. Only perform checks once + // it has happened. + ASSERT_TRUE(server.RunOnce(false)); // Reset requested. + ASSERT_TRUE(server.RunOnce(false)); // Reset happening. + accelerationChecker->performChecks = true; + ASSERT_TRUE(server.RunOnce(false)); // Checks happening. + } + + // Ensure that the right number of checks have been performed. + EXPECT_EQ(accelerationChecker->testCounter, kNumCases); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 9ff752e95a..5f07404905 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -43,17 +43,22 @@ set(cli_sources ) set (sources + Actor.cc Barrier.cc BaseView.cc Conversions.cc + ComponentFactory.cc EntityComponentManager.cc EntityComponentManagerDiff.cc + Joint.cc LevelManager.cc + Light.cc Link.cc Model.cc Primitives.cc SdfEntityCreator.cc SdfGenerator.cc + Sensor.cc Server.cc ServerConfig.cc ServerPrivate.cc @@ -64,7 +69,6 @@ set (sources Util.cc View.cc World.cc - cmd/ModelCommandAPI.cc ${PROTO_PRIVATE_SRC} ${network_sources} ${comms_sources} @@ -72,6 +76,8 @@ set (sources set (gtest_sources ${gtest_sources} + Actor_TEST.cc + AddedMass_TEST.cc Barrier_TEST.cc BaseView_TEST.cc ComponentFactory_TEST.cc @@ -79,11 +85,14 @@ set (gtest_sources Conversions_TEST.cc EntityComponentManager_TEST.cc EventManager_TEST.cc + Joint_TEST.cc + Light_TEST.cc Link_TEST.cc Model_TEST.cc Primitives_TEST.cc SdfEntityCreator_TEST.cc SdfGenerator_TEST.cc + Sensor_TEST.cc ServerConfig_TEST.cc Server_TEST.cc SimulationRunner_TEST.cc @@ -162,7 +171,10 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME} gz-common${GZ_COMMON_VER}::profiler gz-fuel_tools${GZ_FUEL_TOOLS_VER}::gz-fuel_tools${GZ_FUEL_TOOLS_VER} gz-gui${GZ_GUI_VER}::gz-gui${GZ_GUI_VER} + gz-physics${GZ_PHYSICS_VER}::core + gz-rendering${GZ_RENDERING_VER}::core gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER} + gz-transport${GZ_TRANSPORT_VER}::parameters sdformat${SDF_VER}::sdformat${SDF_VER} protobuf::libprotobuf PRIVATE diff --git a/src/ComponentFactory.cc b/src/ComponentFactory.cc new file mode 100644 index 0000000000..d7fa8d38c4 --- /dev/null +++ b/src/ComponentFactory.cc @@ -0,0 +1,25 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "gz/sim/components/Factory.hh" + +using Factory = gz::sim::components::Factory; + +Factory *Factory::Instance() +{ + return common::SingletonT::Instance(); +} diff --git a/src/ComponentFactory_TEST.cc b/src/ComponentFactory_TEST.cc index 0625f39bfc..fdf396a355 100644 --- a/src/ComponentFactory_TEST.cc +++ b/src/ComponentFactory_TEST.cc @@ -68,7 +68,7 @@ TEST_F(ComponentFactoryTest, Register) EXPECT_EQ(registeredCount + 1, ids.size()); EXPECT_NE(ids.end(), std::find(ids.begin(), ids.end(), MyCustom::typeId)); - // Fail to register same component twice + // Registering the component twice doesn't change the number of type ids. factory->Register("gz_sim_components.MyCustom", new components::ComponentDescriptor()); @@ -85,11 +85,8 @@ TEST_F(ComponentFactoryTest, Register) // Unregister factory->Unregister(); - // Check it has no type id yet ids = factory->TypeIds(); - EXPECT_EQ(registeredCount, ids.size()); - EXPECT_EQ(0u, MyCustom::typeId); - EXPECT_EQ("", factory->Name(MyCustom::typeId)); + EXPECT_EQ(registeredCount + 1, ids.size()); } ///////////////////////////////////////////////// diff --git a/src/Conversions.cc b/src/Conversions.cc index f637dd836b..cc76f158a8 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -33,6 +33,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -707,16 +709,7 @@ template<> GZ_SIM_VISIBLE msgs::Inertial gz::sim::convert(const math::Inertiald &_in) { - msgs::Inertial out; - msgs::Set(out.mutable_pose(), _in.Pose()); - out.set_mass(_in.MassMatrix().Mass()); - out.set_ixx(_in.MassMatrix().Ixx()); - out.set_iyy(_in.MassMatrix().Iyy()); - out.set_izz(_in.MassMatrix().Izz()); - out.set_ixy(_in.MassMatrix().Ixy()); - out.set_ixz(_in.MassMatrix().Ixz()); - out.set_iyz(_in.MassMatrix().Iyz()); - return out; + return msgs::Convert(_in); } ////////////////////////////////////////////////// @@ -724,19 +717,7 @@ template<> GZ_SIM_VISIBLE math::Inertiald gz::sim::convert(const msgs::Inertial &_in) { - math::MassMatrix3d massMatrix; - massMatrix.SetMass(_in.mass()); - massMatrix.SetIxx(_in.ixx()); - massMatrix.SetIyy(_in.iyy()); - massMatrix.SetIzz(_in.izz()); - massMatrix.SetIxy(_in.ixy()); - massMatrix.SetIxz(_in.ixz()); - massMatrix.SetIyz(_in.iyz()); - - math::Inertiald out; - out.SetMassMatrix(massMatrix); - out.SetPose(msgs::Convert(_in.pose())); - return out; + return msgs::Convert(_in); } ////////////////////////////////////////////////// @@ -1197,6 +1178,26 @@ msgs::Sensor gz::sim::convert(const sdf::Sensor &_in) << "sensor pointer is null.\n"; } } + // TODO(ahcorde): Enable this code in Harmonic + // else if (_in.Type() == sdf::SensorType::AIR_SPEED) + // { + // if (_in.AirSpeedSensor()) + // { + // msgs::AirSpeedSensor *sensor = out.mutable_air_speed(); + // + // if (_in.AirSpeedSensor()->SpeedNoise().Type() + // != sdf::NoiseType::NONE) + // { + // sim::set(sensor->mutable_speed_noise(), + // _in.AirSpeedSensor()->PressureNoise()); + // } + // } + // else + // { + // gzerr << "Attempting to convert an air speed SDF sensor, but the " + // << "sensor pointer is null.\n"; + // } + // } else if (_in.Type() == sdf::SensorType::IMU) { if (_in.ImuSensor()) @@ -1422,6 +1423,27 @@ sdf::Sensor gz::sim::convert(const msgs::Sensor &_in) out.SetAirPressureSensor(sensor); } + // TODO(ahcorde): Enable this code in Harmonic + // else if (out.Type() == sdf::SensorType::AIR_SPEED) + // { + // sdf::AirSpeed sensor; + // if (_in.has_air_speed()) + // { + // if (_in.air_speed().has_speed_noise()) + // { + // sensor.SetSpeedNoise(sim::convert( + // _in.air_speed().speed_noise())); + // } + // + // } + // else + // { + // gzerr << "Attempting to convert an air speed sensor message, but the " + // << "message does not have an air speed nested message.\n"; + // } + // + // out.SetAirSpeedSensor(sensor); + // } else if (out.Type() == sdf::SensorType::IMU) { sdf::Imu sensor; @@ -1716,6 +1738,63 @@ sdf::ParticleEmitter gz::sim::convert(const msgs::ParticleEmitter &_in) return out; } +////////////////////////////////////////////////// +template<> +GZ_SIM_VISIBLE +msgs::Projector gz::sim::convert(const sdf::Projector &_in) +{ + msgs::Projector out; + out.set_name(_in.Name()); + msgs::Set(out.mutable_pose(), _in.RawPose()); + out.set_near_clip(_in.NearClip()); + out.set_far_clip(_in.FarClip()); + out.set_fov(_in.HorizontalFov().Radian()); + out.set_texture(_in.Texture().empty() ? "" : + asFullPath(_in.Texture(), _in.FilePath())); + + auto header = out.mutable_header()->add_data(); + header->set_key("visibility_flags"); + header->add_value(std::to_string(_in.VisibilityFlags())); + + return out; +} + +////////////////////////////////////////////////// +template<> +GZ_SIM_VISIBLE +sdf::Projector gz::sim::convert(const msgs::Projector &_in) +{ + sdf::Projector out; + out.SetName(_in.name()); + out.SetNearClip(_in.near_clip()); + out.SetFarClip(_in.far_clip()); + out.SetHorizontalFov(math::Angle(_in.fov())); + out.SetTexture(_in.texture()); + out.SetRawPose(msgs::Convert(_in.pose())); + + /// \todo(anyone) add "visibility_flags" field to projector.proto + for (int i = 0; i < _in.header().data_size(); ++i) + { + auto data = _in.header().data(i); + if (data.key() == "visibility_flags" && data.value_size() > 0) + { + try + { + out.SetVisibilityFlags(std::stoul(data.value(0))); + } + catch (...) + { + gzerr << "Failed to parse projector : " + << data.value(0) << ". Using default value: 0xFFFFFFFF." + << std::endl; + out.SetVisibilityFlags(0xFFFFFFFF); + } + } + } + + return out; +} + ////////////////////////////////////////////////// template<> GZ_SIM_VISIBLE diff --git a/src/Conversions_TEST.cc b/src/Conversions_TEST.cc index 93c045e1a9..3d6e3e5861 100644 --- a/src/Conversions_TEST.cc +++ b/src/Conversions_TEST.cc @@ -35,6 +35,7 @@ #include #include #include +#include #include #include #include @@ -1077,6 +1078,40 @@ TEST(Conversions, ParticleEmitter) EXPECT_FLOAT_EQ(emitter2.ScatterRatio(), emitter.ScatterRatio()); } +///////////////////////////////////////////////// +TEST(Conversions, Projector) +{ + sdf::Projector projector; + projector.SetName("my_projector"); + projector.SetNearClip(0.03); + projector.SetFarClip(30); + projector.SetHorizontalFov(math::Angle(0.4)); + projector.SetTexture("projector.png"); + projector.SetVisibilityFlags(0xFF); + + // Convert SDF to a message. + msgs::Projector projectorMsg = convert(projector); + + EXPECT_EQ("my_projector", projectorMsg.name()); + EXPECT_NEAR(0.03, projectorMsg.near_clip(), 1e-3); + EXPECT_NEAR(30, projectorMsg.far_clip(), 1e-3); + EXPECT_NEAR(0.4, projectorMsg.fov(), 1e-3); + EXPECT_EQ("projector.png", projectorMsg.texture()); + + auto header = projectorMsg.header().data(0); + EXPECT_EQ("visibility_flags", header.key()); + EXPECT_EQ(0xFF, std::stoul(header.value(0))); + + // Convert the message back to SDF. + sdf::Projector projector2 = convert(projectorMsg); + EXPECT_EQ(projector2.Name(), projector.Name()); + EXPECT_NEAR(projector2.NearClip(), projector.NearClip(), 1e-3); + EXPECT_NEAR(projector2.FarClip(), projector.FarClip(), 1e-3); + EXPECT_EQ(projector2.HorizontalFov(), projector.HorizontalFov()); + EXPECT_EQ(projector2.Texture(), projector.Texture()); + EXPECT_EQ(projector2.VisibilityFlags(), projector.VisibilityFlags()); +} + ///////////////////////////////////////////////// TEST(Conversions, PluginElement) { diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index cf942fb717..485a5c272d 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -48,7 +48,7 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace components { -using IntComponent = components::Component; +using IntComponent = Component; GZ_SIM_REGISTER_COMPONENT("gz_sim_components.IntComponent", IntComponent) @@ -337,8 +337,8 @@ TEST_P(EntityComponentManagerFixture, auto comp4 = manager.CreateComponent(eIntDouble, DoubleComponent(0.456)); ASSERT_NE(nullptr, comp4); - auto comp5 = manager.CreateComponent(ePose, - components::Pose({1, 2, 3, 0, 0, 0})); + auto comp5 = manager.CreateComponent(ePose, + Pose({1, 2, 3, 0, 0, 0})); ASSERT_NE(nullptr, comp5); auto comp6 = manager.CreateComponent(eCustom, CustomComponent(Custom())); diff --git a/src/EventManager_TEST.cc b/src/EventManager_TEST.cc index ab7e86c9f2..9a7ae2abeb 100644 --- a/src/EventManager_TEST.cc +++ b/src/EventManager_TEST.cc @@ -29,12 +29,16 @@ TEST(EventManager, EmitConnectTest) { EventManager eventManager; + EXPECT_EQ(0u, eventManager.ConnectionCount()); + bool paused1 = false; auto connection1 = eventManager.Connect( [&paused1](bool _paused) { paused1 = _paused; }); + EXPECT_EQ(1u, eventManager.ConnectionCount()); + // Emitting events causes connection callbacks to be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -47,6 +51,8 @@ TEST(EventManager, EmitConnectTest) paused2 = _paused; }); + EXPECT_EQ(2u, eventManager.ConnectionCount()); + // Multiple connections should each be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -58,9 +64,12 @@ TEST(EventManager, EmitConnectTest) // Clearing the ConnectionPtr will cause it to no longer fire. connection1.reset(); + eventManager.Emit(true); EXPECT_EQ(false, paused1); EXPECT_EQ(true, paused2); + + EXPECT_EQ(1u, eventManager.ConnectionCount()); } /// Test that we are able to connect arbitrary events and signal them. diff --git a/src/Joint.cc b/src/Joint.cc new file mode 100644 index 0000000000..8147897e4e --- /dev/null +++ b/src/Joint.cc @@ -0,0 +1,386 @@ +/* + * 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 "gz/sim/components/ChildLinkName.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/JointAxis.hh" +#include "gz/sim/components/JointEffortLimitsCmd.hh" +#include "gz/sim/components/JointForceCmd.hh" +#include "gz/sim/components/JointPosition.hh" +#include "gz/sim/components/JointPositionLimitsCmd.hh" +#include "gz/sim/components/JointPositionReset.hh" +#include "gz/sim/components/JointTransmittedWrench.hh" +#include "gz/sim/components/JointType.hh" +#include "gz/sim/components/JointVelocity.hh" +#include "gz/sim/components/JointVelocityCmd.hh" +#include "gz/sim/components/JointVelocityLimitsCmd.hh" +#include "gz/sim/components/JointVelocityReset.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/ParentLinkName.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/ThreadPitch.hh" + +#include "gz/sim/Joint.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + + +class gz::sim::Joint::Implementation +{ + /// \brief Id of joint entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Joint::Joint(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Joint::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Joint::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Joint::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Joint::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Joint::ParentLinkName( + const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::ChildLinkName( + const EntityComponentManager &_ecm) const +{ + auto child = _ecm.Component(this->dataPtr->id); + + if (!child) + return std::nullopt; + + return std::optional(child->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Joint::ThreadPitch( + const EntityComponentManager &_ecm) const +{ + auto threadPitch = _ecm.Component(this->dataPtr->id); + + if (!threadPitch) + return std::nullopt; + + return std::optional(threadPitch->Data()); +} + +////////////////////////////////////////////////// +std::optional> Joint::Axis( + const EntityComponentManager &_ecm) const +{ + auto axis = _ecm.Component(this->dataPtr->id); + if (!axis) + return std::nullopt; + + std::vector axisVec{axis->Data()}; + + auto axis2 = _ecm.Component(this->dataPtr->id); + if (axis2) + axisVec.push_back(axis2->Data()); + + return std::optional>(axisVec); +} + +////////////////////////////////////////////////// +std::optional Joint::Type( + const EntityComponentManager &_ecm) const +{ + auto jointType = _ecm.Component(this->dataPtr->id); + + if (!jointType) + return std::nullopt; + + return std::optional(jointType->Data()); +} + +////////////////////////////////////////////////// +Entity Joint::SensorByName(const EntityComponentManager &_ecm, + const std::string &_name) const +{ + return _ecm.EntityByComponents( + components::ParentEntity(this->dataPtr->id), + components::Name(_name), + components::Sensor()); +} + +////////////////////////////////////////////////// +std::vector Joint::Sensors(const EntityComponentManager &_ecm) const +{ + return _ecm.EntitiesByComponents( + components::ParentEntity(this->dataPtr->id), + components::Sensor()); +} + +////////////////////////////////////////////////// +uint64_t Joint::SensorCount(const EntityComponentManager &_ecm) const +{ + return this->Sensors(_ecm).size(); +} + +////////////////////////////////////////////////// +void Joint::SetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities) +{ + auto jointVelocityCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityCmd(_velocities)); + } + else + { + jointVelocityCmd->Data() = _velocities; + } +} + +////////////////////////////////////////////////// +void Joint::SetForce(EntityComponentManager &_ecm, + const std::vector &_forces) +{ + auto jointForceCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointForceCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointForceCmd(_forces)); + } + else + { + jointForceCmd->Data() = _forces; + } +} + +////////////////////////////////////////////////// +void Joint::SetVelocityLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointVelocityLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityLimitsCmd(_limits)); + } + else + { + jointVelocityLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::SetEffortLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointEffortLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointEffortLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointEffortLimitsCmd(_limits)); + } + else + { + jointEffortLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::SetPositionLimits(EntityComponentManager &_ecm, + const std::vector &_limits) +{ + auto jointPosLimitsCmd = + _ecm.Component(this->dataPtr->id); + + if (!jointPosLimitsCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointPositionLimitsCmd(_limits)); + } + else + { + jointPosLimitsCmd->Data() = _limits; + } +} + +////////////////////////////////////////////////// +void Joint::ResetVelocity(EntityComponentManager &_ecm, + const std::vector &_velocities) +{ + auto jointVelocityReset = + _ecm.Component(this->dataPtr->id); + + if (!jointVelocityReset) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointVelocityReset(_velocities)); + } + else + { + jointVelocityReset->Data() = _velocities; + } +} + +////////////////////////////////////////////////// +void Joint::ResetPosition(EntityComponentManager &_ecm, + const std::vector &_positions) +{ + auto jointPositionReset = + _ecm.Component(this->dataPtr->id); + + if (!jointPositionReset) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::JointPositionReset(_positions)); + } + else + { + jointPositionReset->Data() = _positions; + } +} + +////////////////////////////////////////////////// +void Joint::EnableVelocityCheck(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +void Joint::EnablePositionCheck(EntityComponentManager &_ecm, bool _enable) + const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +void Joint::EnableTransmittedWrenchCheck(EntityComponentManager &_ecm, + bool _enable) const +{ + enableComponent(_ecm, this->dataPtr->id, + _enable); +} + +////////////////////////////////////////////////// +std::optional> Joint::Velocity( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional> Joint::Position( + const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData( + this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional> Joint::TransmittedWrench( + const EntityComponentManager &_ecm) const +{ + // TransmittedWrench components contains one wrench msg value + // instead of a vector like JointPosition and JointVelocity + // components. + // todo(anyone) change JointTransmittedWrench to store a vector + // of wrench msgs? + // We provide an API that returns a vector which is consistent + // with Velocity and Position accessor functions + auto comp = _ecm.Component( + this->dataPtr->id); + if (!comp) + return std::nullopt; + std::vector wrenchVec{comp->Data()}; + return wrenchVec; +} + +////////////////////////////////////////////////// +std::optional Joint::ParentModel(const EntityComponentManager &_ecm) + const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} diff --git a/src/Joint_TEST.cc b/src/Joint_TEST.cc new file mode 100644 index 0000000000..9994684aa9 --- /dev/null +++ b/src/Joint_TEST.cc @@ -0,0 +1,138 @@ +/* + * 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 "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Joint.hh" +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" + +///////////////////////////////////////////////// +TEST(JointTest, Constructor) +{ + gz::sim::Joint jointNull; + EXPECT_EQ(gz::sim::kNullEntity, jointNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + EXPECT_EQ(id, joint.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Joint jointCopy(joint); // NOLINT + EXPECT_EQ(joint.Entity(), jointCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + gz::sim::Joint jointCopy; + jointCopy = joint; + EXPECT_EQ(joint.Entity(), jointCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + gz::sim::Joint jointMoved(std::move(joint)); + EXPECT_EQ(id, jointMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Joint joint(id); + + gz::sim::Joint jointMoved; + jointMoved = std::move(joint); + EXPECT_EQ(id, jointMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(JointTest, Sensors) +{ + // jointA + // - sensorAA + // - sensorAB + // + // jointC + + gz::sim::EntityComponentManager ecm; + + // Joint A + auto jointAEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointAEntity, gz::sim::components::Joint()); + ecm.CreateComponent(jointAEntity, + gz::sim::components::Name("jointA_name")); + + // Sensor AA - Child of Joint A + auto sensorAAEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorAAEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::Name("sensorAA_name")); + ecm.CreateComponent(sensorAAEntity, + gz::sim::components::ParentEntity(jointAEntity)); + + // Sensor AB - Child of Joint A + auto sensorABEntity = ecm.CreateEntity(); + ecm.CreateComponent(sensorABEntity, gz::sim::components::Sensor()); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::Name("sensorAB_name")); + ecm.CreateComponent(sensorABEntity, + gz::sim::components::ParentEntity(jointAEntity)); + + // Joint C + auto jointCEntity = ecm.CreateEntity(); + ecm.CreateComponent(jointCEntity, gz::sim::components::Joint()); + ecm.CreateComponent(jointCEntity, + gz::sim::components::Name("jointC_name")); + + std::size_t foundSensors = 0; + + gz::sim::Joint jointA(jointAEntity); + auto sensors = jointA.Sensors(ecm); + EXPECT_EQ(2u, sensors.size()); + for (const auto &sensor : sensors) + { + if (sensor == sensorAAEntity || sensor == sensorABEntity) + foundSensors++; + } + EXPECT_EQ(foundSensors, sensors.size()); + + gz::sim::Joint jointC(jointCEntity); + EXPECT_EQ(0u, jointC.Sensors(ecm).size()); +} diff --git a/src/LevelManager.cc b/src/LevelManager.cc index d06b7312ee..f1701ea7a7 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -21,6 +21,7 @@ #include #include +#include #include #include #include @@ -35,6 +36,7 @@ #include "gz/sim/components/Atmosphere.hh" #include "gz/sim/components/Geometry.hh" #include "gz/sim/components/Gravity.hh" +#include "gz/sim/components/Joint.hh" #include "gz/sim/components/Level.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Light.hh" @@ -513,6 +515,21 @@ void LevelManager::ConfigureDefaultLevel() entityNamesInDefault.insert(light->Name()); } } + + // Joints + // We assume no performers are joints + for (uint64_t jointIndex = 0; + jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex) + { + auto joint = this->runner->sdfWorld->JointByIndex(jointIndex); + + if (this->entityNamesInLevels.find(joint->Name()) == + this->entityNamesInLevels.end()) + { + entityNamesInDefault.insert(joint->Name()); + } + } + // Components this->runner->entityCompMgr.CreateComponent( defaultLevel, components::Level()); @@ -852,6 +869,20 @@ void LevelManager::LoadActiveEntities(const std::set &_namesToLoad) } } + // Joints + for (uint64_t jointIndex = 0; + jointIndex < this->runner->sdfWorld->JointCount(); ++jointIndex) + { + auto joint = this->runner->sdfWorld->JointByIndex(jointIndex); + if (_namesToLoad.find(joint->Name()) != _namesToLoad.end()) + { + Entity jointEntity = this->entityCreator->CreateEntities(joint); + + this->entityCreator->SetParent(jointEntity, this->worldEntity); + } + } + + this->activeEntityNames.insert(_namesToLoad.begin(), _namesToLoad.end()); } @@ -892,6 +923,17 @@ void LevelManager::UnloadInactiveEntities( return true; }); + this->runner->entityCompMgr.Each( + [&](const Entity &_entity, const components::Joint *, + const components::Name *_name) -> bool + { + if (_namesToUnload.find(_name->Data()) != _namesToUnload.end()) + { + this->entityCreator->RequestRemoveEntity(_entity, true); + } + return true; + }); + for (const auto &name : _namesToUnload) { this->activeEntityNames.erase(name); diff --git a/src/Light.cc b/src/Light.cc new file mode 100644 index 0000000000..bcec0ca0b2 --- /dev/null +++ b/src/Light.cc @@ -0,0 +1,522 @@ +/* + * 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 "gz/sim/components/Light.hh" +#include "gz/sim/components/LightType.hh" +#include "gz/sim/components/LightCmd.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" + +#include "gz/sim/Light.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + + +class gz::sim::Light::Implementation +{ + /// \brief Id of light entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Light::Light(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Light::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Light::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Light::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Light::Name(const EntityComponentManager &_ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Light::Type( + const EntityComponentManager &_ecm) const +{ + auto lightType = _ecm.Component(this->dataPtr->id); + + if (!lightType) + return std::nullopt; + + return std::optional(lightType->Data()); +} + +////////////////////////////////////////////////// +std::optional Light::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Light::CastShadows( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().CastShadows()); +} + +////////////////////////////////////////////////// +std::optional Light::Intensity( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Intensity()); +} + +////////////////////////////////////////////////// +std::optional Light::Direction( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Direction()); +} + +////////////////////////////////////////////////// +std::optional Light::DiffuseColor( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Diffuse()); +} + +////////////////////////////////////////////////// +std::optional Light::SpecularColor( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().Specular()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationRange( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().AttenuationRange()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationConstant( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().ConstantAttenuationFactor()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationLinear( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().LinearAttenuationFactor()); +} + +////////////////////////////////////////////////// +std::optional Light::AttenuationQuadratic( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().QuadraticAttenuationFactor()); +} + +////////////////////////////////////////////////// +std::optional Light::SpotInnerAngle( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().SpotInnerAngle()); +} + +////////////////////////////////////////////////// +std::optional Light::SpotOuterAngle( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().SpotOuterAngle()); +} + +////////////////////////////////////////////////// +std::optional Light::SpotFalloff( + const EntityComponentManager &_ecm) const +{ + auto light = _ecm.Component(this->dataPtr->id); + + if (!light) + return std::nullopt; + + return std::optional(light->Data().SpotFalloff()); +} + +////////////////////////////////////////////////// +void Light::SetPose(EntityComponentManager &_ecm, + const math::Pose3d &_pose) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_pose(), _pose); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetCastShadows(EntityComponentManager &_ecm, + bool _castShadows) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_cast_shadows(_castShadows); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetIntensity(EntityComponentManager &_ecm, + double _intensity) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_intensity(_intensity); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetDirection(EntityComponentManager &_ecm, + const math::Vector3d &_dir) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_direction(), _dir); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetDiffuseColor(EntityComponentManager &_ecm, + const math::Color &_color) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_diffuse(), _color); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpecularColor(EntityComponentManager &_ecm, + const math::Color &_color) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + msgs::Set(lightMsg.mutable_specular(), _color); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetAttenuationRange(EntityComponentManager &_ecm, + double _range) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_range(_range); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetAttenuationConstant(EntityComponentManager &_ecm, + double _value) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_attenuation_constant(_value); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} +////////////////////////////////////////////////// +void Light::SetAttenuationLinear(EntityComponentManager &_ecm, + double _value) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_attenuation_linear(_value); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetAttenuationQuadratic(EntityComponentManager &_ecm, + double _value) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_attenuation_quadratic(_value); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpotInnerAngle(EntityComponentManager &_ecm, + const math::Angle &_angle) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_spot_inner_angle(_angle.Radian()); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpotOuterAngle(EntityComponentManager &_ecm, + const math::Angle &_angle) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_spot_outer_angle(_angle.Radian()); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +void Light::SetSpotFalloff(EntityComponentManager &_ecm, + double _falloff) +{ + auto lightCmd = + _ecm.Component(this->dataPtr->id); + + msgs::Light lightMsg; + lightMsg.set_spot_falloff(_falloff); + if (!lightCmd) + { + _ecm.CreateComponent( + this->dataPtr->id, + components::LightCmd(lightMsg)); + } + else + { + lightCmd->Data() = lightMsg; + } +} + +////////////////////////////////////////////////// +std::optional Light::Parent(const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} diff --git a/src/Light_TEST.cc b/src/Light_TEST.cc new file mode 100644 index 0000000000..0d3023367c --- /dev/null +++ b/src/Light_TEST.cc @@ -0,0 +1,83 @@ +/* + * 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 "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Light.hh" +#include "gz/sim/components/Light.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" + +///////////////////////////////////////////////// +TEST(LightTest, Constructor) +{ + gz::sim::Light lightNull; + EXPECT_EQ(gz::sim::kNullEntity, lightNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Light light(id); + + EXPECT_EQ(id, light.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Light lightCopy(light); // NOLINT + EXPECT_EQ(light.Entity(), lightCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + gz::sim::Light lightCopy; + lightCopy = light; + EXPECT_EQ(light.Entity(), lightCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + gz::sim::Light lightMoved(std::move(light)); + EXPECT_EQ(id, lightMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(LightTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Light light(id); + + gz::sim::Light lightMoved; + lightMoved = std::move(light); + EXPECT_EQ(id, lightMoved.Entity()); +} diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index cc6b374980..fba9663f3e 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -34,6 +34,7 @@ #else #include "gz/sim/components/Actor.hh" #include "gz/sim/components/AirPressureSensor.hh" +#include "gz/sim/components/AirSpeedSensor.hh" #include "gz/sim/components/Altimeter.hh" #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/Atmosphere.hh" @@ -69,11 +70,12 @@ #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/NavSat.hh" -#include "gz/sim/components/ParentLinkName.hh" #include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/ParentLinkName.hh" #include #include "gz/sim/components/Physics.hh" #include "gz/sim/components/Pose.hh" +#include #include "gz/sim/components/RgbdCamera.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/SegmentationCamera.hh" @@ -528,6 +530,16 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Actor *_actor) this->dataPtr->ecm->CreateComponent(actorEntity, components::Name(_actor->Name())); + // Links + for (uint64_t linkIndex = 0; linkIndex < _actor->LinkCount(); + ++linkIndex) + { + auto link = _actor->LinkByIndex(linkIndex); + auto linkEntity = this->CreateEntities(link); + + this->SetParent(linkEntity, actorEntity); + } + // Actor plugins this->dataPtr->eventManager->Emit(actorEntity, _actor->Plugins()); @@ -641,7 +653,7 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) } // Particle emitters - for (uint64_t emitterIndex = 0; emitterIndex < _link->ParticleEmitterCount(); + for (uint64_t emitterIndex = 0; emitterIndex < _link->ParticleEmitterCount(); ++emitterIndex) { auto emitter = _link->ParticleEmitterByIndex(emitterIndex); @@ -650,6 +662,17 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link) this->SetParent(emitterEntity, linkEntity); } + // Projectors + for (uint64_t projectorIndex = 0; projectorIndex < _link->ProjectorCount(); + ++projectorIndex) + { + auto projector = _link->ProjectorByIndex(projectorIndex); + auto projectorEntity = this->CreateEntities(projector); + + this->SetParent(projectorEntity, linkEntity); + } + + return linkEntity; } @@ -860,6 +883,25 @@ Entity SdfEntityCreator::CreateEntities(const sdf::ParticleEmitter *_emitter) return emitterEntity; } +////////////////////////////////////////////////// +Entity SdfEntityCreator::CreateEntities(const sdf::Projector *_projector) +{ + GZ_PROFILE("SdfEntityCreator::CreateEntities(sdf::Projector)"); + + // Entity + Entity projectorEntity = this->dataPtr->ecm->CreateEntity(); + + // Components + this->dataPtr->ecm->CreateComponent(projectorEntity, + components::Projector(*_projector)); + this->dataPtr->ecm->CreateComponent(projectorEntity, + components::Pose(ResolveSdfPose(_projector->SemanticPose()))); + this->dataPtr->ecm->CreateComponent(projectorEntity, + components::Name(_projector->Name())); + + return projectorEntity; +} + ////////////////////////////////////////////////// Entity SdfEntityCreator::CreateEntities(const sdf::Collision *_collision) { @@ -961,6 +1003,19 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::WorldPose(math::Pose3d::Zero)); } + else if (_sensor->Type() == sdf::SensorType::AIR_SPEED) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::AirSpeedSensor(*_sensor)); + + // create components to be filled by physics + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldPose(math::Pose3d::Zero)); + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldLinearVelocity(math::Vector3d::Zero)); + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldAngularVelocity(math::Vector3d::Zero)); + } else if (_sensor->Type() == sdf::SensorType::ALTIMETER) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index 7b364e211a..05184eed57 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -27,6 +27,7 @@ #include "gz/sim/Util.hh" #include "gz/sim/components/AirPressureSensor.hh" +#include "gz/sim/components/AirSpeedSensor.hh" #include "gz/sim/components/Altimeter.hh" #include "gz/sim/components/Camera.hh" #include "gz/sim/components/ChildLinkName.hh" @@ -724,6 +725,15 @@ namespace sdf_generator _elem->Copy(sensor.ToElement()); return updateSensorNameAndPose(); } + // air speed + auto airSpeedComp = + _ecm.Component(_entity); + if (airSpeedComp) + { + const sdf::Sensor &sensor = airSpeedComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } // force torque auto forceTorqueComp = _ecm.Component(_entity); if (forceTorqueComp) diff --git a/src/SdfGenerator_TEST.cc b/src/SdfGenerator_TEST.cc index 465e3b9e2f..8e032ca2d4 100644 --- a/src/SdfGenerator_TEST.cc +++ b/src/SdfGenerator_TEST.cc @@ -514,7 +514,10 @@ TEST_F(ElementUpdateFixture, ConfigOverride) auto uri = inclElem->Get("uri"); EXPECT_FALSE(uri.empty()); const std::string version = common::split(uri, "/").back(); - EXPECT_NO_THROW(std::stol(version)); + + int64_t versionParsed = -1; + EXPECT_NO_THROW(versionParsed = std::stol(version)); + EXPECT_NE(-1, versionParsed); } } diff --git a/src/Sensor.cc b/src/Sensor.cc new file mode 100644 index 0000000000..e9c9939f07 --- /dev/null +++ b/src/Sensor.cc @@ -0,0 +1,101 @@ +/* + * 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 "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" + +#include "gz/sim/Sensor.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; + + +class gz::sim::Sensor::Implementation +{ + /// \brief Id of sensor entity. + public: sim::Entity id{kNullEntity}; +}; + +////////////////////////////////////////////////// +Sensor::Sensor(sim::Entity _entity) + : dataPtr(utils::MakeImpl()) +{ + this->dataPtr->id = _entity; +} + +////////////////////////////////////////////////// +sim::Entity Sensor::Entity() const +{ + return this->dataPtr->id; +} + +////////////////////////////////////////////////// +void Sensor::ResetEntity(sim::Entity _newEntity) +{ + this->dataPtr->id = _newEntity; +} + +////////////////////////////////////////////////// +bool Sensor::Valid(const EntityComponentManager &_ecm) const +{ + return nullptr != _ecm.Component(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Sensor::Name(const EntityComponentManager &_ecm) + const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +std::optional Sensor::Pose( + const EntityComponentManager &_ecm) const +{ + auto pose = _ecm.Component(this->dataPtr->id); + + if (!pose) + return std::nullopt; + + return std::optional(pose->Data()); +} + +////////////////////////////////////////////////// +std::optional Sensor::Topic( + const EntityComponentManager &_ecm) const +{ + auto topic = _ecm.Component(this->dataPtr->id); + + if (!topic) + return std::nullopt; + + return std::optional(topic->Data()); +} + +////////////////////////////////////////////////// +std::optional Sensor::Parent(const EntityComponentManager &_ecm) const +{ + auto parent = _ecm.Component(this->dataPtr->id); + + if (!parent) + return std::nullopt; + + return std::optional(parent->Data()); +} diff --git a/src/Sensor_TEST.cc b/src/Sensor_TEST.cc new file mode 100644 index 0000000000..5c07367a0e --- /dev/null +++ b/src/Sensor_TEST.cc @@ -0,0 +1,82 @@ +/* + * 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 "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Sensor.hh" + +///////////////////////////////////////////////// +TEST(SensorTest, Constructor) +{ + gz::sim::Sensor sensorNull; + EXPECT_EQ(gz::sim::kNullEntity, sensorNull.Entity()); + + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + EXPECT_EQ(id, sensor.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, CopyConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + // Marked nolint because we are specifically testing copy + // constructor here (clang wants unnecessary copies removed) + gz::sim::Sensor sensorCopy(sensor); // NOLINT + EXPECT_EQ(sensor.Entity(), sensorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, CopyAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + gz::sim::Sensor sensorCopy; + sensorCopy = sensor; + EXPECT_EQ(sensor.Entity(), sensorCopy.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, MoveConstructor) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + gz::sim::Sensor sensorMoved(std::move(sensor)); + EXPECT_EQ(id, sensorMoved.Entity()); +} + +///////////////////////////////////////////////// +TEST(SensorTest, MoveAssignmentOperator) +{ + gz::sim::Entity id(3); + gz::sim::Sensor sensor(id); + + gz::sim::Sensor sensorMoved; + sensorMoved = std::move(sensor); + EXPECT_EQ(id, sensorMoved.Entity()); +} diff --git a/src/ServerConfig.cc b/src/ServerConfig.cc index 0e715cb87b..030edc6a00 100644 --- a/src/ServerConfig.cc +++ b/src/ServerConfig.cc @@ -248,6 +248,7 @@ class gz::sim::ServerConfigPrivate : sdfFile(_cfg->sdfFile), sdfString(_cfg->sdfString), updateRate(_cfg->updateRate), + initialSimTime(_cfg->initialSimTime), useLevels(_cfg->useLevels), useLogRecord(_cfg->useLogRecord), logRecordPath(_cfg->logRecordPath), @@ -277,6 +278,9 @@ class gz::sim::ServerConfigPrivate /// \brief An optional update rate. public: std::optional updateRate; + /// \brief The initial simulation time in seconds. + public: double initialSimTime = 0; + /// \brief Use the level system public: bool useLevels{false}; @@ -399,6 +403,12 @@ std::string ServerConfig::SdfString() const return this->dataPtr->sdfString; } +////////////////////////////////////////////////// +void ServerConfig::SetInitialSimTime(const double &_initialSimTime) const +{ + this->dataPtr->initialSimTime = _initialSimTime; +} + ////////////////////////////////////////////////// void ServerConfig::SetUpdateRate(const double &_hz) { @@ -406,6 +416,12 @@ void ServerConfig::SetUpdateRate(const double &_hz) this->dataPtr->updateRate = _hz; } +///////////////////////////////////////////////// +double ServerConfig::InitialSimTime() const +{ + return this->dataPtr->initialSimTime; +} + ///////////////////////////////////////////////// std::optional ServerConfig::UpdateRate() const { diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 86d620711f..f19915dcab 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -71,6 +71,10 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, // Keep world name this->worldName = _world->Name(); + this->parametersRegistry = std::make_unique< + gz::transport::parameters::ParametersRegistry>( + std::string{"world/"} + this->worldName); + // Get the physics profile // TODO(luca): remove duplicated logic in SdfEntityCreator and LevelManager auto physics = _world->PhysicsByIndex(0); @@ -122,6 +126,12 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, static_cast(this->stepSize.count() / this->desiredRtf)); } + // Epoch + this->simTimeEpoch = std::chrono::round( + std::chrono::duration{_config.InitialSimTime()} + ); + this->currentInfo.simTime = this->simTimeEpoch; + // World control transport::NodeOptions opts; std::string ns{"/world/" + this->worldName}; @@ -142,8 +152,9 @@ SimulationRunner::SimulationRunner(const sdf::World *_world, this->node = std::make_unique(opts); // Create the system manager - this->systemMgr = std::make_unique(_systemLoader, - &this->entityCompMgr, &this->eventMgr, validNs); + this->systemMgr = std::make_unique( + _systemLoader, &this->entityCompMgr, &this->eventMgr, validNs, + this->parametersRegistry.get()); this->pauseConn = this->eventMgr.Connect( std::bind(&SimulationRunner::SetPaused, this, std::placeholders::_1)); @@ -266,13 +277,11 @@ void SimulationRunner::UpdateCurrentInfo() // Rewind if (this->requestedRewind) { - gzdbg << "Rewinding simulation back to time zero." << std::endl; - this->realTimes.clear(); - this->simTimes.clear(); + gzdbg << "Rewinding simulation back to initial time." << std::endl; this->realTimeFactor = 0; - this->currentInfo.dt = -this->currentInfo.simTime; - this->currentInfo.simTime = std::chrono::steady_clock::duration::zero(); + this->currentInfo.dt = this->simTimeEpoch - this->currentInfo.simTime; + this->currentInfo.simTime = this->simTimeEpoch; this->currentInfo.realTime = std::chrono::steady_clock::duration::zero(); this->currentInfo.iterations = 0; this->realTimeWatch.Reset(); @@ -285,63 +294,32 @@ void SimulationRunner::UpdateCurrentInfo() } // Seek - if (this->requestedSeek >= std::chrono::steady_clock::duration::zero()) + if (this->requestedSeek && this->requestedSeek.value() >= this->simTimeEpoch) { gzdbg << "Seeking to " << std::chrono::duration_cast( - this->requestedSeek).count() << "s." << std::endl; + this->requestedSeek.value()).count() << "s." << std::endl; - this->realTimes.clear(); - this->simTimes.clear(); this->realTimeFactor = 0; - this->currentInfo.dt = this->requestedSeek - this->currentInfo.simTime; - this->currentInfo.simTime = this->requestedSeek; + this->currentInfo.dt = this->requestedSeek.value() - + this->currentInfo.simTime; + this->currentInfo.simTime = this->requestedSeek.value(); this->currentInfo.iterations = 0; this->currentInfo.realTime = this->realTimeWatch.ElapsedRunTime(); - this->requestedSeek = std::chrono::steady_clock::duration{-1}; + this->requestedSeek = {}; return; } // Regular time flow - // Store the real time and sim time only if not paused. - if (this->realTimeWatch.Running()) - { - this->realTimes.push_back(this->realTimeWatch.ElapsedRunTime()); - this->simTimes.push_back(this->currentInfo.simTime); - } - - // Maintain a window size of 20 for realtime and simtime. - if (this->realTimes.size() > 20) - this->realTimes.pop_front(); - if (this->simTimes.size() > 20) - this->simTimes.pop_front(); + const double simTimeCount = + static_cast(this->currentInfo.simTime.count()); + const double realTimeCount = + static_cast(this->currentInfo.realTime.count()); - // Compute the average sim and real times. - std::chrono::steady_clock::duration simAvg{0}, realAvg{0}; - std::list::iterator simIter, - realIter; - - simIter = ++(this->simTimes.begin()); - realIter = ++(this->realTimes.begin()); - while (simIter != this->simTimes.end() && realIter != this->realTimes.end()) - { - simAvg += ((*simIter) - this->simTimes.front()); - realAvg += ((*realIter) - this->realTimes.front()); - ++simIter; - ++realIter; - } - - // RTF, only compute this if the realTime count is greater than zero. The - // realtTime count could be zero if simulation was started paused. - if (realAvg.count() > 0) - { - this->realTimeFactor = math::precision( - static_cast(simAvg.count()) / realAvg.count(), 4); - } // Fill the current update info this->currentInfo.realTime = this->realTimeWatch.ElapsedRunTime(); @@ -356,6 +334,15 @@ void SimulationRunner::UpdateCurrentInfo() ++this->currentInfo.iterations; this->currentInfo.dt = this->stepSize; } + const double simTimeDiff = + static_cast(this->currentInfo.simTime.count()) - simTimeCount; + const double realTimeDiff = + static_cast(this->currentInfo.realTime.count()) - realTimeCount; + + if (realTimeDiff > 0) + { + this->realTimeFactor = simTimeDiff / realTimeDiff; + } } ///////////////////////////////////////////////// @@ -401,8 +388,6 @@ void SimulationRunner::UpdatePhysicsParams() } if (updated) { - this->simTimes.clear(); - this->realTimes.clear(); // Set as OneTimeChange to make sure the update is not missed this->entityCompMgr.SetChanged(worldEntity, components::Physics::typeId, ComponentState::OneTimeChange); @@ -846,13 +831,12 @@ void SimulationRunner::Step(const UpdateInfo &_info) // Update all the systems. this->UpdateSystems(); - if (!this->Paused() && - this->requestedRunToSimTime > - std::chrono::steady_clock::duration::zero() && - this->currentInfo.simTime >= this->requestedRunToSimTime) + if (!this->Paused() && this->requestedRunToSimTime && + this->requestedRunToSimTime.value() > this->simTimeEpoch && + this->currentInfo.simTime >= this->requestedRunToSimTime.value()) { this->SetPaused(true); - this->requestedRunToSimTime = std::chrono::steady_clock::duration{-1}; + this->requestedRunToSimTime = {}; } if (!this->Paused() && this->pendingSimIterations > 0) @@ -1106,14 +1090,13 @@ bool SimulationRunner::Stepping() const void SimulationRunner::SetRunToSimTime( const std::chrono::steady_clock::duration &_time) { - if (_time >= std::chrono::steady_clock::duration::zero() && - _time > this->currentInfo.simTime) + if (_time >= this->simTimeEpoch && _time > this->currentInfo.simTime) { this->requestedRunToSimTime = _time; } else { - this->requestedRunToSimTime = std::chrono::seconds(-1); + this->requestedRunToSimTime = {}; } } @@ -1255,7 +1238,7 @@ void SimulationRunner::ProcessWorldControl() this->requestedRewind = control.rewind; // Seek - if (control.seek >= std::chrono::steady_clock::duration::zero()) + if (control.seek >= this->simTimeEpoch) { this->requestedSeek = control.seek; } @@ -1357,6 +1340,13 @@ const UpdateInfo &SimulationRunner::CurrentInfo() const return this->currentInfo; } +///////////////////////////////////////////////// +const std::chrono::steady_clock::duration & + SimulationRunner::SimTimeEpoch() const +{ + return this->simTimeEpoch; +} + ///////////////////////////////////////////////// const std::chrono::steady_clock::duration & SimulationRunner::UpdatePeriod() const diff --git a/src/SimulationRunner.hh b/src/SimulationRunner.hh index 8690d81168..438fc329ba 100644 --- a/src/SimulationRunner.hh +++ b/src/SimulationRunner.hh @@ -192,6 +192,10 @@ namespace gz public: void SetUpdatePeriod( const std::chrono::steady_clock::duration &_updatePeriod); + /// \brief Get the simulation epoch. + /// \return The simulation epoch. + public: const std::chrono::steady_clock::duration &SimTimeEpoch() const; + /// \brief Get the update period. /// \return The update period. public: const std::chrono::steady_clock::duration &UpdatePeriod() const; @@ -217,8 +221,8 @@ namespace gz /// \brief Set the run to simulation time. /// \param[in] _time A simulation time in the future to run to and then - /// pause. A negative number or a time less than the current simulation - /// time disables the run-to feature. + /// pause. A time prior than the current simulation time disables the + /// run-to feature. public: void SetRunToSimTime( const std::chrono::steady_clock::duration &_time); @@ -400,6 +404,11 @@ namespace gz /// Note: must be before EntityComponentManager private: EventManager eventMgr; + /// \brief Manager all parameters + private: std::unique_ptr< + gz::transport::parameters::ParametersRegistry + > parametersRegistry; + /// \brief Manager of all components. private: EntityComponentManager entityCompMgr; @@ -427,11 +436,10 @@ namespace gz /// The default update rate is 500hz, which is a period of 2ms. private: std::chrono::steady_clock::duration updatePeriod{2ms}; - /// \brief List of simulation times used to compute averages. - private: std::list simTimes; + /// \brief The simulation epoch. + /// All simulation times will be larger than the epoch. It defaults to 0. + private: std::chrono::steady_clock::duration simTimeEpoch{0}; - /// \brief List of real times used to compute averages. - private: std::list realTimes; /// \brief Node for communication. private: std::unique_ptr node{nullptr}; @@ -484,12 +492,12 @@ namespace gz private: bool requestedRewind{false}; /// \brief If user asks to seek to a specific sim time, this holds the - /// time.s A negative value means there's no request from the user. - private: std::chrono::steady_clock::duration requestedSeek{-1}; + /// time. + private: std::optional requestedSeek; - /// \brief A simulation time in the future to run to and then pause. - /// A negative number indicates that this variable it not being used. - private: std::chrono::steady_clock::duration requestedRunToSimTime{-1}; + /// \brief A simulation time past the epoch to run to and then pause. + private: std::optional + requestedRunToSimTime; /// \brief Keeps the latest simulation info. private: UpdateInfo currentInfo; diff --git a/src/SystemInternal.hh b/src/SystemInternal.hh index 5aeb4b7278..0f4d442e4d 100644 --- a/src/SystemInternal.hh +++ b/src/SystemInternal.hh @@ -46,6 +46,8 @@ namespace gz : systemPlugin(std::move(_systemPlugin)), system(systemPlugin->QueryInterface()), configure(systemPlugin->QueryInterface()), + configureParameters( + systemPlugin->QueryInterface()), reset(systemPlugin->QueryInterface()), preupdate(systemPlugin->QueryInterface()), update(systemPlugin->QueryInterface()), @@ -62,6 +64,8 @@ namespace gz : systemShared(_system), system(_system.get()), configure(dynamic_cast(_system.get())), + configureParameters( + dynamic_cast(_system.get())), reset(dynamic_cast(_system.get())), preupdate(dynamic_cast(_system.get())), update(dynamic_cast(_system.get())), @@ -86,6 +90,11 @@ namespace gz /// Will be nullptr if the System doesn't implement this interface. public: ISystemConfigure *configure = nullptr; + /// \brief Access this system via the ISystemConfigureParameters + /// interface. + /// Will be nullptr if the System doesn't implement this interface. + public: ISystemConfigureParameters *configureParameters = nullptr; + /// \brief Access this system via the ISystemReset interface /// Will be nullptr if the System doesn't implement this interface. public: ISystemReset *reset = nullptr; diff --git a/src/SystemManager.cc b/src/SystemManager.cc index 72a66960ce..fd43f5330d 100644 --- a/src/SystemManager.cc +++ b/src/SystemManager.cc @@ -28,13 +28,16 @@ using namespace gz; using namespace sim; ////////////////////////////////////////////////// -SystemManager::SystemManager(const SystemLoaderPtr &_systemLoader, - EntityComponentManager *_entityCompMgr, - EventManager *_eventMgr, - const std::string &_namespace) +SystemManager::SystemManager( + const SystemLoaderPtr &_systemLoader, + EntityComponentManager *_entityCompMgr, + EventManager *_eventMgr, + const std::string &_namespace, + gz::transport::parameters::ParametersRegistry *_parametersRegistry) : systemLoader(_systemLoader), entityCompMgr(_entityCompMgr), - eventMgr(_eventMgr) + eventMgr(_eventMgr), + parametersRegistry(_parametersRegistry) { transport::NodeOptions opts; opts.SetNameSpace(_namespace); @@ -106,6 +109,9 @@ size_t SystemManager::ActivatePendingSystems() if (system.configure) this->systemsConfigure.push_back(system.configure); + if (system.configureParameters) + this->systemsConfigureParameters.push_back(system.configureParameters); + if (system.reset) this->systemsReset.push_back(system.reset); @@ -255,6 +261,16 @@ void SystemManager::AddSystemImpl( *this->eventMgr); } + // Configure the system parameters, if necessary + if ( + _system.configureParameters && this->entityCompMgr && + this->parametersRegistry) + { + _system.configureParameters->ConfigureParameters( + *this->parametersRegistry, + *this->entityCompMgr); + } + // Update callbacks will be handled later, add to queue std::lock_guard lock(this->pendingSystemsMutex); this->pendingSystems.push_back(_system); @@ -266,6 +282,12 @@ const std::vector& SystemManager::SystemsConfigure() return this->systemsConfigure; } +const std::vector& +SystemManager::SystemsConfigureParameters() +{ + return this->systemsConfigureParameters; +} + ////////////////////////////////////////////////// const std::vector &SystemManager::SystemsReset() { diff --git a/src/SystemManager.hh b/src/SystemManager.hh index 8a44f2bd4e..acd82c09dc 100644 --- a/src/SystemManager.hh +++ b/src/SystemManager.hh @@ -52,10 +52,13 @@ namespace gz /// \param[in] _eventMgr Pointer to the event manager to be used when /// configuring new systems /// \param[in] _namespace Namespace to use for the transport node - public: explicit SystemManager(const SystemLoaderPtr &_systemLoader, - EntityComponentManager *_entityCompMgr = nullptr, - EventManager *_eventMgr = nullptr, - const std::string &_namespace = std::string()); + public: explicit SystemManager( + const SystemLoaderPtr &_systemLoader, + EntityComponentManager *_entityCompMgr = nullptr, + EventManager *_eventMgr = nullptr, + const std::string &_namespace = std::string(), + gz::transport::parameters::ParametersRegistry * + _parametersRegistry = nullptr); /// \brief Load system plugin for a given entity. /// \param[in] _entity Entity @@ -113,6 +116,12 @@ namespace gz /// \return Vector of systems' configure interfaces. public: const std::vector& SystemsConfigure(); + /// \brief Get an vector of all active systems implementing + /// "ConfigureParameters" + /// \return Vector of systems's configure interfaces. + public: const std::vector& + SystemsConfigureParameters(); + /// \brief Get an vector of all active systems implementing "Reset" /// \return Vector of systems' reset interfaces. public: const std::vector& SystemsReset(); @@ -180,6 +189,10 @@ namespace gz /// \brief Systems implementing Configure private: std::vector systemsConfigure; + /// \brief Systems implementing ConfigureParameters + private: std::vector + systemsConfigureParameters; + /// \brief Systems implementing Reset private: std::vector systemsReset; @@ -212,6 +225,10 @@ namespace gz /// \brief Node for communication. private: std::unique_ptr node{nullptr}; + + /// \brief Pointer to associated parameters registry + private: gz::transport::parameters::ParametersRegistry * + parametersRegistry; }; } } // namespace sim diff --git a/src/SystemManager_TEST.cc b/src/SystemManager_TEST.cc index e9e636815a..5026842a9f 100644 --- a/src/SystemManager_TEST.cc +++ b/src/SystemManager_TEST.cc @@ -31,7 +31,8 @@ using namespace gz::sim; ///////////////////////////////////////////////// class SystemWithConfigure: public System, - public ISystemConfigure + public ISystemConfigure, + public ISystemConfigureParameters { // Documentation inherited public: void Configure( @@ -40,7 +41,12 @@ class SystemWithConfigure: EntityComponentManager &, EventManager &) override { configured++; }; + public: void ConfigureParameters( + gz::transport::parameters::ParametersRegistry &, + EntityComponentManager &) override { configuredParameters++; } + public: int configured = 0; + public: int configuredParameters = 0; }; ///////////////////////////////////////////////// @@ -99,6 +105,7 @@ TEST(SystemManager, AddSystemNoEcm) // SystemManager without an ECM/EventmManager will mean no config occurs EXPECT_EQ(0, configSystem->configured); + EXPECT_EQ(0, configSystem->configuredParameters); EXPECT_EQ(0u, systemMgr.ActiveCount()); EXPECT_EQ(1u, systemMgr.PendingCount()); @@ -150,7 +157,10 @@ TEST(SystemManager, AddSystemEcm) auto ecm = EntityComponentManager(); auto eventManager = EventManager(); - SystemManager systemMgr(loader, &ecm, &eventManager); + auto paramRegistry = std::make_unique< + gz::transport::parameters::ParametersRegistry>("SystemManager_TEST"); + SystemManager systemMgr( + loader, &ecm, &eventManager, std::string(), paramRegistry.get()); EXPECT_EQ(0u, systemMgr.ActiveCount()); EXPECT_EQ(0u, systemMgr.PendingCount()); @@ -165,6 +175,7 @@ TEST(SystemManager, AddSystemEcm) // Configure called during AddSystem EXPECT_EQ(1, configSystem->configured); + EXPECT_EQ(1, configSystem->configuredParameters); EXPECT_EQ(0u, systemMgr.ActiveCount()); EXPECT_EQ(1u, systemMgr.PendingCount()); diff --git a/src/Util.cc b/src/Util.cc index c2424ebf62..b8d658cf96 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -15,6 +15,8 @@ * */ +#include + #include #include #include @@ -29,6 +31,7 @@ #include #include "gz/sim/components/Actor.hh" +#include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/Collision.hh" #include "gz/sim/components/Environment.hh" #include "gz/sim/components/Joint.hh" @@ -38,11 +41,13 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParticleEmitter.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/SphericalCoordinates.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/World.hh" +#include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/Util.hh" @@ -81,6 +86,45 @@ math::Pose3d worldPose(const Entity &_entity, return pose; } +////////////////////////////////////////////////// +math::Vector3d relativeVel(const Entity &_entity, + const EntityComponentManager &_ecm) +{ + auto poseComp = _ecm.Component(_entity); + if (nullptr == poseComp) + { + gzwarn << "Trying to get world pose from entity [" << _entity + << "], which doesn't have a pose component" << std::endl; + return math::Vector3d(); + } + + // work out pose in world frame + math::Pose3d pose = poseComp->Data(); + auto p = _ecm.Component(_entity); + while (p) + { + // get pose of parent entity + auto parentPose = _ecm.Component(p->Data()); + if (!parentPose) + break; + // transform pose + pose = parentPose->Data() * pose; + // keep going up the tree + p = _ecm.Component(p->Data()); + } + + auto worldLinVel = _ecm.Component(_entity); + if (nullptr == worldLinVel) + { + gzwarn << "Trying to get world velocity from entity [" << _entity + << "], which doesn't have a velocity component" << std::endl; + return math::Vector3d(); + } + + math::Vector3d vel = worldLinVel->Data(); + return pose.Rot().RotateVectorReverse(vel); +} + ////////////////////////////////////////////////// std::string scopedName(const Entity &_entity, const EntityComponentManager &_ecm, const std::string &_delim, @@ -234,6 +278,10 @@ ComponentTypeId entityTypeId(const Entity &_entity, { type = components::ParticleEmitter::typeId; } + else if (_ecm.Component(_entity)) + { + type = components::Projector::typeId; + } return type; } @@ -284,6 +332,10 @@ std::string entityTypeStr(const Entity &_entity, { type = "particle_emitter"; } + else if (_ecm.Component(_entity)) + { + type = "projector"; + } return type; } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 09cf777907..9b291983f6 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -32,6 +32,7 @@ #include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParticleEmitter.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/Visual.hh" #include "gz/sim/components/World.hh" @@ -360,6 +361,10 @@ TEST_F(UtilTest, EntityTypeId) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::ParticleEmitter()); EXPECT_EQ(components::ParticleEmitter::typeId, entityTypeId(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Projector()); + EXPECT_EQ(components::Projector::typeId, entityTypeId(entity, ecm)); } ///////////////////////////////////////////////// @@ -409,6 +414,10 @@ TEST_F(UtilTest, EntityTypeStr) entity = ecm.CreateEntity(); ecm.CreateComponent(entity, components::ParticleEmitter()); EXPECT_EQ("particle_emitter", entityTypeStr(entity, ecm)); + + entity = ecm.CreateEntity(); + ecm.CreateComponent(entity, components::Projector()); + EXPECT_EQ("projector", entityTypeStr(entity, ecm)); } ///////////////////////////////////////////////// diff --git a/src/cmd/ModelCommandAPI.hh b/src/cmd/ModelCommandAPI.hh index c749be820e..485c630a89 100644 --- a/src/cmd/ModelCommandAPI.hh +++ b/src/cmd/ModelCommandAPI.hh @@ -15,10 +15,10 @@ * */ -#include "gz/sim/Export.hh" +#include "gz/sim/gz/Export.hh" /// \brief External hook to get a list of available models. -extern "C" GZ_SIM_VISIBLE void cmdModelList(); +extern "C" GZ_SIM_GZ_VISIBLE void cmdModelList(); /// \brief External hook to dump model information. /// \param[in] _modelName Model name. @@ -26,7 +26,7 @@ extern "C" GZ_SIM_VISIBLE void cmdModelList(); /// \param[in] _linkName Link name. /// \param[in] _jointName Joint name. /// \param[in] _sensorName Sensor name. -extern "C" GZ_SIM_VISIBLE void cmdModelInfo( +extern "C" GZ_SIM_GZ_VISIBLE void cmdModelInfo( const char *_modelName, int _pose, const char *_linkName, const char *_jointName, const char *_sensorName); diff --git a/src/cmd/cmdsim.rb.in b/src/cmd/cmdsim.rb.in index ca6f2ad174..64ee638c7b 100755 --- a/src/cmd/cmdsim.rb.in +++ b/src/cmd/cmdsim.rb.in @@ -49,6 +49,8 @@ COMMANDS = { 'sim' => "Available Options: \n"\ " -g Run only the GUI. \n"\ "\n"\ + " --initial-sim-time [arg] Initial simulation time, in seconds. \n"\ + "\n"\ " --iterations [arg] Number of iterations to execute. \n"\ "\n"\ " --levels Use the level system. The default is false, \n"\ @@ -230,6 +232,7 @@ class Cmd 'file' => '', 'gui' => 0, 'hz' => -1, + 'initial_sim_time' => 0, 'iterations' => 0, 'levels' => 0, 'network_role' => '', @@ -278,6 +281,10 @@ class Cmd opts.on('-z [arg]', Float, 'Update rate in Hertz') do |h| options['hz'] = h end + opts.on('--initial-sim-time [arg]', Float, + 'Initial simulation time, in seconds.') do |t| + options['initial_sim_time'] = t + end opts.on('-r') do options['run'] = 1 end @@ -444,7 +451,7 @@ has properly set the DYLD_LIBRARY_PATH environment variables." parsed = '' if options['file'] != '' # Check if the passed in file exists. - if File.exists?(options['file']) + if File.exist?(options['file']) path = options['file'] # If not, then first check the GZ_SIM_RESOURCE_PATH environment # variable, then the configuration path from the launch library. @@ -465,7 +472,7 @@ Please use [GZ_SIM_RESOURCE_PATH] instead." resourcePaths = resourcePathEnv.split(':') for resourcePath in resourcePaths filePath = File.join(resourcePath, options['file']) - if File.exists?(filePath) + if File.exist?(filePath) path = filePath break end @@ -475,7 +482,7 @@ Please use [GZ_SIM_RESOURCE_PATH] instead." if path.nil? Importer.extern 'char *worldInstallDir()' path = File.join(Importer.worldInstallDir().to_s, options['file']) - if !File.exists?(path) + if !File.exist?(path) path = Importer.findFuelResource(options['file']).to_s options['file'] = path if path == "" @@ -491,7 +498,7 @@ Please use [GZ_SIM_RESOURCE_PATH] instead." end # Import the runServer function - Importer.extern 'int runServer(const char *, int, int, float, int, + Importer.extern 'int runServer(const char *, int, int, float, double, int, const char *, int, int, const char *, int, int, int, const char *, const char *, const char *, const char *, const char *, @@ -530,12 +537,14 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info." ENV['RMT_PORT'] = '1500' Process.setpgid(0, 0) Process.setproctitle('gz sim server') - Importer.runServer(parsed, options['iterations'], options['run'], - options['hz'], options['levels'], options['network_role'], - options['network_secondaries'], options['record'], - options['record-path'], options['record-resources'], - options['log-overwrite'], options['log-compress'], - options['playback'], options['physics_engine'], + Importer.runServer(parsed, + options['iterations'], options['run'], options['hz'], + options['initial_sim_time'], options['levels'], + options['network_role'], options['network_secondaries'], + options['record'], options['record-path'], + options['record-resources'], options['log-overwrite'], + options['log-compress'], options['playback'], + options['physics_engine'], options['render_engine_server'], options['render_engine_server_api_backend'], options['render_engine_gui'], @@ -574,11 +583,12 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info." elsif options['server'] == 1 ENV['RMT_PORT'] = '1500' Importer.runServer(parsed, options['iterations'], options['run'], - options['hz'], options['levels'], options['network_role'], - options['network_secondaries'], options['record'], - options['record-path'], options['record-resources'], - options['log-overwrite'], options['log-compress'], - options['playback'], options['physics_engine'], + options['hz'], options['initial_sim_time'], options['levels'], + options['network_role'], options['network_secondaries'], + options['record'], options['record-path'], + options['record-resources'], options['log-overwrite'], + options['log-compress'], options['playback'], + options['physics_engine'], options['render_engine_server'], options['render_engine_server_api_backend'], options['render_engine_gui'], @@ -599,10 +609,6 @@ See https://github.com/gazebosim/gz-sim/issues/168 for more info." options['wait_gui'], options['render_engine_gui'], options['render_engine_gui_api_backend']) end - rescue - puts "Library error: Problem running [#{options['command']}]() "\ - "from #{plugin}." - # begin end # execute end diff --git a/src/gui/CMakeLists.txt b/src/gui/CMakeLists.txt index 3437aff68f..0f774b50f2 100644 --- a/src/gui/CMakeLists.txt +++ b/src/gui/CMakeLists.txt @@ -76,3 +76,7 @@ gz_build_tests(TYPE UNIT gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-transport${GZ_TRANSPORT_VER}::gz-transport${GZ_TRANSPORT_VER} ) + +if(TARGET UNIT_Gui_clean_exit_TEST) + set_tests_properties(UNIT_Gui_clean_exit_TEST PROPERTIES TIMEOUT 300) +endif() diff --git a/src/gui/Gui_clean_exit_TEST.cc b/src/gui/Gui_clean_exit_TEST.cc index 8e86a0ce44..4a96d17f1b 100644 --- a/src/gui/Gui_clean_exit_TEST.cc +++ b/src/gui/Gui_clean_exit_TEST.cc @@ -69,7 +69,7 @@ void startBoth(const std::string &_fileName) // SIGTERM during initialization doesn't always work properly and that is not // what we are testing here. using namespace std::chrono_literals; - std::this_thread::sleep_for(4s); + std::this_thread::sleep_for(15s); std::raise(SIGTERM); serverThread.join(); guiThread.join(); diff --git a/src/gui/plugins/component_inspector/CMakeLists.txt b/src/gui/plugins/component_inspector/CMakeLists.txt index 53119092b1..5eca36cc5b 100644 --- a/src/gui/plugins/component_inspector/CMakeLists.txt +++ b/src/gui/plugins/component_inspector/CMakeLists.txt @@ -1,10 +1,12 @@ gz_add_gui_plugin(ComponentInspector SOURCES ComponentInspector.cc + Inertial.cc Pose3d.cc SystemPluginInfo.cc QT_HEADERS ComponentInspector.hh + Inertial.hh Pose3d.hh SystemPluginInfo.hh ) diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc index 85898e5617..2f8fed736d 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.cc +++ b/src/gui/plugins/component_inspector/ComponentInspector.cc @@ -79,6 +79,7 @@ #include "gz/sim/gui/GuiEvents.hh" #include "ComponentInspector.hh" +#include "Inertial.hh" #include "Pose3d.hh" #include "SystemPluginInfo.hh" @@ -120,6 +121,9 @@ namespace gz::sim public: std::map updateViewCbs; + /// \brief Handles all Inertial components. + public: std::unique_ptr inertial; + /// \brief Handles all components displayed as a 3D pose. public: std::unique_ptr pose3d; @@ -481,6 +485,7 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *) "ComponentsModel", &this->dataPtr->componentsModel); // Type-specific handlers + this->dataPtr->inertial = std::make_unique(this); this->dataPtr->pose3d = std::make_unique(this); this->dataPtr->systemInfo = std::make_unique(this); @@ -1034,8 +1039,8 @@ void ComponentInspector::OnLight( double _outerAngle, double _falloff, double _intensity, int _type, bool _isLightOn, bool _visualizeVisual) { - std::function cb = - [](const gz::msgs::Boolean &/*_rep*/, const bool _result) + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) gzerr << "Error setting light configuration" << std::endl; diff --git a/src/gui/plugins/component_inspector/ComponentInspector.qrc b/src/gui/plugins/component_inspector/ComponentInspector.qrc index 24b9a85cfb..68531631f2 100644 --- a/src/gui/plugins/component_inspector/ComponentInspector.qrc +++ b/src/gui/plugins/component_inspector/ComponentInspector.qrc @@ -4,6 +4,7 @@ ComponentInspector.qml ExpandingTypeHeader.qml Float.qml + Inertial.qml Integer.qml Light.qml NoData.qml diff --git a/src/gui/plugins/component_inspector/Inertial.cc b/src/gui/plugins/component_inspector/Inertial.cc new file mode 100644 index 0000000000..9cc9ec44f3 --- /dev/null +++ b/src/gui/plugins/component_inspector/Inertial.cc @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ + +#include +#include + +#include "Inertial.hh" + +using namespace gz; +using namespace sim; +using namespace inspector; + +///////////////////////////////////////////////// +Inertial::Inertial(ComponentInspector *_inspector) +{ + _inspector->Context()->setContextProperty("InertialImpl", this); + this->inspector = _inspector; + + this->inspector->AddUpdateViewCb(components::Inertial::typeId, + std::bind(&Inertial::UpdateView, this, + std::placeholders::_1, std::placeholders::_2)); +} + +///////////////////////////////////////////////// +void Inertial::UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item) +{ + auto comp = _ecm.Component( + this->inspector->GetEntity()); + if (nullptr == _item || nullptr == comp) + return; + + auto inertial = comp->Data(); + + _item->setData(QString("Inertial"), + ComponentsModel::RoleNames().key("dataType")); + + QList dataList = { + QVariant(inertial.Pose().Pos().X()), + QVariant(inertial.Pose().Pos().Y()), + QVariant(inertial.Pose().Pos().Z()), + QVariant(inertial.Pose().Rot().Roll()), + QVariant(inertial.Pose().Rot().Pitch()), + QVariant(inertial.Pose().Rot().Yaw()), + QVariant(inertial.MassMatrix().Mass()), + QVariant(inertial.MassMatrix().Ixx()), + QVariant(inertial.MassMatrix().Ixy()), + QVariant(inertial.MassMatrix().Ixz()), + QVariant(inertial.MassMatrix().Iyy()), + QVariant(inertial.MassMatrix().Iyz()), + QVariant(inertial.MassMatrix().Izz()) + }; + + if (inertial.FluidAddedMass().has_value()) + { + dataList.append(QVariant(inertial.FluidAddedMass().value()(0, 0))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(0, 1))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(0, 2))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(0, 3))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(0, 4))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(0, 5))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(1, 1))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(1, 2))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(1, 3))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(1, 4))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(1, 5))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(2, 2))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(2, 3))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(2, 4))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(2, 5))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(3, 3))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(3, 4))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(3, 5))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(4, 4))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(4, 5))); + dataList.append(QVariant(inertial.FluidAddedMass().value()(5, 5))); + } + + _item->setData(dataList, ComponentsModel::RoleNames().key("data")); +} diff --git a/src/gui/plugins/component_inspector/Inertial.hh b/src/gui/plugins/component_inspector/Inertial.hh new file mode 100644 index 0000000000..9d55cce115 --- /dev/null +++ b/src/gui/plugins/component_inspector/Inertial.hh @@ -0,0 +1,58 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_SIM_GUI_COMPONENTINSPECTOR_INERTIAL_HH_ +#define GZ_SIM_GUI_COMPONENTINSPECTOR_INERTIAL_HH_ + +#include + +#include "gz/sim/components/Inertial.hh" +#include "gz/sim/EntityComponentManager.hh" + +#include "ComponentInspector.hh" +#include "Types.hh" + +#include + +namespace gz +{ +namespace sim +{ +class ComponentInspector; +namespace inspector +{ + /// \brief A class that handles Inertial components. + class Inertial : public QObject + { + Q_OBJECT + + /// \brief Constructor + /// \param[in] _inspector The component inspector. + public: explicit Inertial(ComponentInspector *_inspector); + + /// \brief Callback when there are ECM updates. + /// \param[in] _ecm Immutable reference to the ECM. + /// \param[in] _item Item to update. + public: void UpdateView(const EntityComponentManager &_ecm, + QStandardItem *_item); + + /// \brief Pointer to the component inspector. + private: ComponentInspector *inspector{nullptr}; + }; +} +} +} +#endif diff --git a/src/gui/plugins/component_inspector/Inertial.qml b/src/gui/plugins/component_inspector/Inertial.qml new file mode 100644 index 0000000000..7ddaf2e4ad --- /dev/null +++ b/src/gui/plugins/component_inspector/Inertial.qml @@ -0,0 +1,669 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 1.4 +import QtQuick.Controls 2.2 +import QtQuick.Controls.Material 2.1 +import QtQuick.Layouts 1.3 +import QtQuick.Controls.Styles 1.4 +import "qrc:/ComponentInspector" + +// TODO +// * IgnHelpers decimal +// * Reduce duplication on numbers + +// Item displaying inertial information. +Rectangle { + height: header.height + content.height + width: componentInspector.width + color: index % 2 == 0 ? lightGrey : darkGrey + + // Left indentation + property int indentation: 10 + + // Horizontal margins + property int margin: 5 + + // Light text color according to theme + property string propertyColor: Material.theme == Material.Light ? "#444444" : "#bbbbbb" + + // Dark text color according to theme + property string valueColor: Material.theme == Material.Light ? "black" : "white" + + // True if inertial has added mass + property bool hasAddedMass: componentData.length === 34 + + Column { + anchors.fill: parent + + ExpandingTypeHeader { + id: header + // Using the default header text values. + } + + // Content + Rectangle { + id: content + property bool show: false + width: parent.width + height: show ? column.height : 0 + clip: true + color: "transparent" + + Behavior on height { + NumberAnimation { + duration: 200; + easing.type: Easing.InOutQuad + } + } + + Column { + id: column + width: parent.width - indentation - 2 * margin + leftPadding: indentation + margin + rightPadding: margin + + // Mass + Row { + width: parent.width + height: massText.height + + Text { + id : massText + text: 'Mass (kg) ' + leftPadding: margin + color: propertyColor + font.pointSize: 12 + } + Text { + id: massValue + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: valueColor + font.pointSize: 12 + text: { + var decimals = getDecimals(massValue.width) + var valueText = componentData ? componentData[6].toFixed(decimals) : "N/A" + return valueText + } + } + } + + // Pose + Text { + text: 'Pose' + leftPadding: margin + color: propertyColor + font.pointSize: 12 + } + // TODO pose + + // Inertia + Text { + text: 'Inertia' + leftPadding: margin + color: propertyColor + font.pointSize: 12 + } + + GridLayout { + width: parent.width + columns: 6 + + // Left spacer + Item { + Layout.rowSpan: 4 + width: margin + indentation + } + + Item { + width: margin + } + + Text { + text: 'x' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'y' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'z' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + // Right spacer + Item { + Layout.rowSpan: 4 + width: margin + indentation + } + + Text { + text: 'x' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[7] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[8] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[9] + } + + Text { + text: 'y' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[8] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[10] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[11] + } + + Text { + text: 'z' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[9] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[11] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[12] + } + } + + // Added mass + Text { + text: 'Fluid added mass' + leftPadding: margin + visible: hasAddedMass + color: propertyColor + font.pointSize: 12 + } + + GridLayout { + width: parent.width + columns: 7 + visible: hasAddedMass + + Item { + width: margin + } + + Text { + text: 'x' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'y' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'z' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'p' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'q' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'r' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + text: 'x' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[13] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[14] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[15] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[16] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[17] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[18] + } + + Text { + text: 'y' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[14] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[19] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[20] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[21] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[22] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[23] + } + + Text { + text: 'z' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[15] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[20] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[24] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[25] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[26] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[27] + } + + Text { + text: 'p' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[16] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[21] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[25] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[28] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[29] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[30] + } + + Text { + text: 'q' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[17] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[22] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[26] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[29] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[31] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[32] + } + + Text { + text: 'r' + horizontalAlignment: Text.AlignRight + Layout.fillWidth: true + color: propertyColor + font.pointSize: 10 + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[18] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[23] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[27] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[30] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[32] + } + + Text { + Layout.fillWidth: true + horizontalAlignment: Text.AlignRight + color: valueColor + font.pointSize: 12 + text: componentData[33] + } + } + } + } + } +} diff --git a/src/gui/plugins/modules/EntityContextMenu.cc b/src/gui/plugins/modules/EntityContextMenu.cc index bee1896e98..812a787540 100644 --- a/src/gui/plugins/modules/EntityContextMenu.cc +++ b/src/gui/plugins/modules/EntityContextMenu.cc @@ -202,31 +202,31 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) } else if (request == "view_transparent") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewTransparentService, req, cb); } else if (request == "view_com") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewCOMService, req, cb); } else if (request == "view_inertia") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewInertiaService, req, cb); } else if (request == "view_joints") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewJointsService, req, cb); } else if (request == "view_wireframes") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->viewWireframesService, req, cb); } @@ -244,13 +244,13 @@ void EntityContextMenu::OnRequest(const QString &_request, const QString &_data) } else if (request == "copy") { - gz::msgs::StringMsg req; + msgs::StringMsg req; req.set_data(_data.toStdString()); this->dataPtr->node.Request(this->dataPtr->copyService, req, cb); } else if (request == "paste") { - gz::msgs::Empty req; + msgs::Empty req; this->dataPtr->node.Request(this->dataPtr->pasteService, req, cb); } else diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 77178315c3..e268d5f05b 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -552,6 +552,28 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) } auto servers = this->dataPtr->fuelClient->Config().Servers(); + // Since the ign->gz rename, `servers` here returns two items for the + // canonical Fuel server: fuel.ignitionrobotics.org and fuel.gazebosim.org. + // For the purposes of the ResourceSpawner, these will be treated as the same + // and we will remove the ignitionrobotics server here. + auto urlIs = [](const std::string &_url) + { + return [_url](const fuel_tools::ServerConfig &_server) + { return _server.Url().Str() == _url; }; + }; + + auto ignIt = std::find_if(servers.begin(), servers.end(), + urlIs("https://fuel.ignitionrobotics.org")); + if (ignIt != servers.end()) + { + auto gzsimIt = std::find_if(servers.begin(), servers.end(), + urlIs("https://fuel.gazebosim.org")); + if (gzsimIt != servers.end()) + { + servers.erase(ignIt); + } + } + gzmsg << "Please wait... Loading models from Fuel.\n"; // Add notice for the user that fuel resources are being loaded @@ -572,7 +594,7 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) } // Create each fuel resource and add them to the ownerModelMap - for (auto id : models) + for (const auto &id : models) { Resource resource; resource.name = id.Name(); diff --git a/src/gui/plugins/spawn/Spawn.cc b/src/gui/plugins/spawn/Spawn.cc index fc01427d2c..4d2a492343 100644 --- a/src/gui/plugins/spawn/Spawn.cc +++ b/src/gui/plugins/spawn/Spawn.cc @@ -146,6 +146,9 @@ namespace gz::sim /// \brief Text for popup error message public: QString errorPopupText; + + /// \brief Adds new line after each nChar. + public: std::string AddNewLine(const std::string &_str, int _nChar); }; } @@ -188,6 +191,22 @@ void Spawn::LoadConfig(const tinyxml2::XMLElement *) ()->installEventFilter(this); } +///////////////////////////////////////////////// +std::string SpawnPrivate::AddNewLine(const std::string &_str, int _nChar) +{ + std::string out; + out.reserve(_str.size() + _str.size() / _nChar); + for (std::string::size_type i = 0; i < _str.size(); i++) + { + if (!(i % _nChar) && i) + { + out.append("-\n"); + } + out.push_back(_str[i]); + } + return out; +} + ///////////////////////////////////////////////// void SpawnPrivate::HandlePlacement() { @@ -581,10 +600,12 @@ void Spawn::OnDropped(const gz::gui::events::DropOnScene *_event) if (!common::MeshManager::Instance()->IsValidFilename(dropStr)) { - QString errTxt = QString::fromStdString("Invalid URI: " + dropStr + - "\nOnly Fuel URLs or mesh file types DAE, FBX, GLTF, OBJ, and STL are " - + "supported."); - this->SetErrorPopupText(errTxt); + std::string fixedDropStr = this->dataPtr->AddNewLine(dropStr, 55); + std::string errTxt = "Invalid URI: " + fixedDropStr + + "\nOnly Fuel URLs or mesh file types DAE, FBX, GLTF, OBJ, and STL\n" + "are supported."; + QString QErrTxt = QString::fromStdString(errTxt); + this->SetErrorPopupText(QErrTxt); return; } diff --git a/src/gui/plugins/transform_control/TransformControl.cc b/src/gui/plugins/transform_control/TransformControl.cc index 669c44f33b..e95eeadc47 100644 --- a/src/gui/plugins/transform_control/TransformControl.cc +++ b/src/gui/plugins/transform_control/TransformControl.cc @@ -56,6 +56,9 @@ namespace gz::sim /// \brief Perform transformations in the render thread. public: void HandleTransform(); + /// \brief Handle mouse events + public: void HandleMouseEvents(); + /// \brief Snaps a point at intervals of a fixed distance. Currently used /// to give a snapping behavior when moving models with a mouse. /// \param[in] _point Input point to snap. @@ -494,9 +497,21 @@ void TransformControlPrivate::HandleTransform() // update gizmo visual this->transformControl.Update(); + this->HandleMouseEvents(); + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); +} + +///////////////////////////////////////////////// +void TransformControlPrivate::HandleMouseEvents() +{ // check for mouse events if (!this->mouseDirty) return; + this->mouseDirty = false; // handle mouse movements if (this->mouseEvent.Button() == gz::common::MouseEvent::LEFT) @@ -534,7 +549,6 @@ void TransformControlPrivate::HandleTransform() // It's ok to get here } } - this->mouseDirty = false; } else { @@ -601,7 +615,6 @@ void TransformControlPrivate::HandleTransform() } this->transformControl.Stop(); - this->mouseDirty = false; } // Select entity else if (!this->mouseEvent.Dragging()) @@ -672,7 +685,6 @@ void TransformControlPrivate::HandleTransform() } } - this->mouseDirty = false; return; } } @@ -829,13 +841,7 @@ void TransformControlPrivate::HandleTransform() } this->transformControl.Scale(scale); } - this->mouseDirty = false; } - - gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); - gz::gui::App()->sendEvent( - gz::gui::App()->findChild(), - &blockOrbitEvent); } ///////////////////////////////////////////////// diff --git a/src/gui/plugins/view_angle/ViewAngle.cc b/src/gui/plugins/view_angle/ViewAngle.cc index 13bf3068a5..d1f3d5518e 100644 --- a/src/gui/plugins/view_angle/ViewAngle.cc +++ b/src/gui/plugins/view_angle/ViewAngle.cc @@ -30,6 +30,7 @@ #include #include #include +#include #include #include #include @@ -57,16 +58,21 @@ namespace gz::sim public: std::mutex mutex; /// \brief View Control service name - public: std::string viewControlService; + public: std::string viewControlService = "/gui/camera/view_control"; /// \brief View Control reference visual service name - public: std::string viewControlRefVisualService; + public: std::string viewControlRefVisualService = + "/gui/camera/view_control/reference_visual"; + + /// \brief View Control sensitivity service name + public: std::string viewControlSensitivityService = + "/gui/camera/view_control/sensitivity"; /// \brief Move gui camera to pose service name - public: std::string moveToPoseService; + public: std::string moveToPoseService = "/gui/move_to/pose"; /// \brief Move gui camera to model service name - public: std::string moveToModelService; + public: std::string moveToModelService = "/gui/move_to/model"; /// \brief New move to model message public: bool newMoveToModel = false; @@ -74,6 +80,13 @@ namespace gz::sim /// \brief Distance of the camera to the model public: double distanceMoveToModel = 0.0; + /// \brief Camera horizontal fov + public: double horizontalFov = 0.0; + + /// \brief Flag indicating if there is a new camera horizontalFOV + /// coming from qml side + public: bool newHorizontalFOV = false; + /// \brief gui camera pose public: math::Pose3d camPose; @@ -97,6 +110,11 @@ namespace gz::sim /// \return True if there is a new view controller from gui camera public: bool UpdateQtViewControl(); + /// \brief Checks if there is new camera horizontal fov from gui camera, + /// used to update qml side + /// \return True if there is a new horizontal fov from gui camera + public: bool UpdateQtCamHorizontalFOV(); + /// \brief User camera public: rendering::CameraPtr camera{nullptr}; @@ -141,26 +159,16 @@ void ViewAngle::LoadConfig(const tinyxml2::XMLElement *) if (this->title.empty()) this->title = "View Angle"; - // view control requests - this->dataPtr->viewControlService = "/gui/camera/view_control"; - - // view control reference visual requests - this->dataPtr->viewControlRefVisualService = "/gui/camera/reference_visual"; - // Subscribe to camera pose std::string topic = "/gui/camera/pose"; this->dataPtr->node.Subscribe( topic, &ViewAngle::CamPoseCb, this); - // Move to pose service - this->dataPtr->moveToPoseService = "/gui/move_to/pose"; - // Move to model service - this->dataPtr->moveToModelService = "/gui/move_to/model"; this->dataPtr->node.Advertise(this->dataPtr->moveToModelService, &ViewAngle::OnMoveToModelService, this); - ignmsg << "Move to model service on [" - << this->dataPtr->moveToModelService << "]" << std::endl; + gzmsg << "Move to model service on [" + << this->dataPtr->moveToModelService << "]" << std::endl; gz::gui::App()->findChild< gz::gui::MainWindow *>()->installEventFilter(this); @@ -179,6 +187,12 @@ bool ViewAngle::eventFilter(QObject *_obj, QEvent *_event) this->CamClipDistChanged(); } + // updates qml cam horizontal fov spin boxes if changed + if (this->dataPtr->UpdateQtCamHorizontalFOV()) + { + this->CamHorizontalFOVChanged(); + } + if (this->dataPtr->UpdateQtViewControl()) { this->ViewControlIndexChanged(); @@ -252,7 +266,7 @@ void ViewAngle::OnViewControlReferenceVisual(bool _enable) [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) - ignerr << "Error setting view controller reference visual" << std::endl; + gzerr << "Error setting view controller reference visual" << std::endl; }; msgs::Boolean req; @@ -262,6 +276,29 @@ void ViewAngle::OnViewControlReferenceVisual(bool _enable) this->dataPtr->viewControlRefVisualService, req, cb); } +///////////////////////////////////////////////// +void ViewAngle::OnViewControlSensitivity(double _sensitivity) +{ + std::function cb = + [](const msgs::Boolean &/*_rep*/, const bool _result) + { + if (!_result) + gzerr << "Error setting view controller sensitivity" << std::endl; + }; + + if (_sensitivity <= 0.0) + { + gzerr << "View controller sensitivity must be greater than 0" << std::endl; + return; + } + + msgs::Double req; + req.set_data(_sensitivity); + + this->dataPtr->node.Request( + this->dataPtr->viewControlSensitivityService, req, cb); +} + ///////////////////////////////////////////////// QList ViewAngle::CamPose() const { @@ -293,8 +330,8 @@ bool ViewAngle::OnMoveToModelService(const gz::msgs::GUICamera &_msg, auto visualToMove = scene->VisualByName(_msg.name()); if (nullptr == visualToMove) { - ignerr << "Failed to get visual with ID [" - << _msg.name() << "]" << std::endl; + gzerr << "Failed to get visual with ID [" + << _msg.name() << "]" << std::endl; _res.set_data(false); return false; } @@ -307,8 +344,8 @@ bool ViewAngle::OnMoveToModelService(const gz::msgs::GUICamera &_msg, } catch(std::bad_variant_access &_e) { - ignerr << "Failed to get gazebo-entity user data [" - << visualToMove->Name() << "]" << std::endl; + gzerr << "Failed to get gazebo-entity user data [" + << visualToMove->Name() << "]" << std::endl; _res.set_data(false); return false; } @@ -327,7 +364,7 @@ bool ViewAngle::OnMoveToModelService(const gz::msgs::GUICamera &_msg, [](const msgs::Boolean &/*_rep*/, const bool _result) { if (!_result) - ignerr << "Error setting view controller" << std::endl; + gzerr << "Error setting view controller" << std::endl; }; msgs::StringMsg req; @@ -344,7 +381,7 @@ bool ViewAngle::OnMoveToModelService(const gz::msgs::GUICamera &_msg, } else { - ignerr << "Unknown view controller selected: " << str << std::endl; + gzerr << "Unknown view controller selected: " << str << std::endl; _res.set_data(false); return false; } @@ -374,6 +411,19 @@ void ViewAngle::CamPoseCb(const msgs::Pose &_msg) } } +///////////////////////////////////////////////// +double ViewAngle::HorizontalFOV() const +{ + return this->dataPtr->horizontalFov; +} + +///////////////////////////////////////////////// +void ViewAngle::SetHorizontalFOV(double _horizontalFOV) +{ + this->dataPtr->horizontalFov = _horizontalFOV; + this->dataPtr->newHorizontalFOV = true; +} + ///////////////////////////////////////////////// QList ViewAngle::CamClipDist() const { @@ -507,6 +557,13 @@ void ViewAnglePrivate::OnRender() this->camera->SetFarClipPlane(this->camClipDist[1]); this->newCamClipDist = false; } + + // Camera horizontalFOV + if (this->newHorizontalFOV) + { + this->camera->SetHFOV(gz::math::Angle(this->horizontalFov)); + this->newHorizontalFOV = false; + } } ///////////////////////////////////////////////// @@ -545,6 +602,18 @@ void ViewAnglePrivate::OnComplete() } } +///////////////////////////////////////////////// +bool ViewAnglePrivate::UpdateQtCamHorizontalFOV() +{ + bool updated = false; + if (std::abs(this->camera->HFOV().Radian() - this->horizontalFov) > 0.0001) + { + this->horizontalFov = this->camera->HFOV().Radian(); + updated = true; + } + return updated; +} + ///////////////////////////////////////////////// bool ViewAnglePrivate::UpdateQtCamClipDist() { diff --git a/src/gui/plugins/view_angle/ViewAngle.hh b/src/gui/plugins/view_angle/ViewAngle.hh index 6b3e0fda16..134f2e2b6d 100644 --- a/src/gui/plugins/view_angle/ViewAngle.hh +++ b/src/gui/plugins/view_angle/ViewAngle.hh @@ -62,6 +62,13 @@ namespace sim NOTIFY ViewControlIndexChanged ) + /// \brief gui camera horizontal fov + Q_PROPERTY( + double horizontalFOV + READ HorizontalFOV + NOTIFY CamHorizontalFOVChanged + ) + /// \brief Constructor public: ViewAngle(); @@ -93,6 +100,20 @@ namespace sim /// false to hide it public slots: void OnViewControlReferenceVisual(bool _enable); + /// \brief Callback in Qt thread when camera view controller changes. + /// \param[in] _sensitivity View control sensitivity vlaue + public slots: void OnViewControlSensitivity(double _sensitivity); + + /// \brief Updates gui camera's Horizontal fov + /// \param[in] _horizontalFOV Horizontal fov + public slots: void SetHorizontalFOV(double _horizontalFOV); + + /// \brief Get the current gui horizontal fov. + public: Q_INVOKABLE double HorizontalFOV() const; + + /// \brief Notify that the gui camera's horizontal fov changed + signals: void CamHorizontalFOVChanged(); + /// \brief Get the current gui camera pose. public: Q_INVOKABLE QList CamPose() const; diff --git a/src/gui/plugins/view_angle/ViewAngle.qml b/src/gui/plugins/view_angle/ViewAngle.qml index 3f69bea5a6..36eab6d999 100644 --- a/src/gui/plugins/view_angle/ViewAngle.qml +++ b/src/gui/plugins/view_angle/ViewAngle.qml @@ -24,7 +24,7 @@ import "qrc:/qml" ColumnLayout { Layout.minimumWidth: 320 - Layout.minimumHeight: 530 + Layout.minimumHeight: 650 anchors.fill: parent ToolBar { @@ -207,6 +207,30 @@ ColumnLayout { } } + // view control sensitivity + GridLayout { + Layout.fillWidth: true + Layout.margins: 10 + columns: 2 + + Label { + id: viewControlSensitivityLabel + text: "View control sensitivity" + } + GzSpinBox { + id: viewControlSensitivitySpinBox + Layout.fillWidth: true + value: 1.0 + maximumValue: 10.0 + minimumValue: 0.01 + decimals: 2 + stepSize: 0.1 + onEditingFinished:{ + ViewAngle.OnViewControlSensitivity(value) + } + } + } + // toggle view control reference visual CheckBox { Layout.alignment: Qt.AlignHCenter @@ -302,6 +326,41 @@ ColumnLayout { } } + // Set camera's horizontal FOV + Text { + text: "Horizontal FOV" + Layout.fillWidth: true + color: Material.Grey + leftPadding: 5 + topPadding: 10 + font.bold: true + } + + GridLayout { + width: parent.width + columns: 2 + + Text { + text: "HorizontalFOV (rads)" + color: "dimgrey" + Layout.row: 0 + Layout.column: 0 + leftPadding: 5 + } + GzSpinBox { + id: horizontalFOV + Layout.fillWidth: true + Layout.row: 0 + Layout.column: 1 + value: ViewAngle.horizontalFOV + maximumValue: 3.14159 + minimumValue: 0.000001 + decimals: 6 + stepSize: 0.01 + onEditingFinished: ViewAngle.SetHorizontalFOV(horizontalFOV.value) + } + } + // Bottom spacer Item { width: 10 diff --git a/src/gui/resources/GazeboDrawer.qml b/src/gui/resources/GazeboDrawer.qml index c860934e74..6047b66c5a 100644 --- a/src/gui/resources/GazeboDrawer.qml +++ b/src/gui/resources/GazeboDrawer.qml @@ -152,6 +152,7 @@ Rectangle { focus: true parent: ApplicationWindow.overlay width: parent.width / 3 > 500 ? 500 : parent.width / 3 + height: 300 x: (parent.width - width) / 2 y: (parent.height - height) / 2 closePolicy: Popup.CloseOnEscape @@ -261,6 +262,7 @@ Rectangle { modal: true focus: true parent: ApplicationWindow.overlay + width: messageText.implicitWidth x: (parent.width - width) / 2 y: (parent.height - height) / 2 closePolicy: Popup.CloseOnEscape diff --git a/src/gz.cc b/src/gz.cc index d627a5a42f..7d355424e0 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -130,7 +130,8 @@ extern "C" const char *findFuelResource( ////////////////////////////////////////////////// extern "C" int runServer(const char *_sdfString, - int _iterations, int _run, float _hz, int _levels, const char *_networkRole, + int _iterations, int _run, float _hz, double _initialSimTime, + int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, int _logCompress, const char *_playback, const char *_physicsEngine, @@ -351,6 +352,9 @@ extern "C" int runServer(const char *_sdfString, else serverConfig.SetSdfFile(_file); + // Initial simulation time. + serverConfig.SetInitialSimTime(_initialSimTime); + // Set the update rate. if (_hz > 0.0) serverConfig.SetUpdateRate(_hz); diff --git a/src/gz.hh b/src/gz.hh index 7251cb6980..d03b37c6e5 100644 --- a/src/gz.hh +++ b/src/gz.hh @@ -17,22 +17,22 @@ #ifndef GZ_SIM_GZ_HH_ #define GZ_SIM_GZ_HH_ -#include "gz/sim/Export.hh" +#include "gz/sim/gz/Export.hh" /// \brief External hook to read the library version. /// \return C-string representing the version. Ex.: 0.1.2 -extern "C" GZ_SIM_VISIBLE char *gzSimVersion(); +extern "C" GZ_SIM_GZ_VISIBLE char *gzSimVersion(); /// \brief Get the Gazebo version header. /// \return C-string containing the Gazebo version information. -extern "C" GZ_SIM_VISIBLE char *simVersionHeader(); +extern "C" GZ_SIM_GZ_VISIBLE char *simVersionHeader(); /// \brief Set verbosity level /// \param[in] _verbosity 0 to 4 -extern "C" GZ_SIM_VISIBLE void cmdVerbosity( +extern "C" GZ_SIM_GZ_VISIBLE void cmdVerbosity( const char *_verbosity); -extern "C" GZ_SIM_VISIBLE const char *worldInstallDir(); +extern "C" GZ_SIM_GZ_VISIBLE const char *worldInstallDir(); /// \brief External hook to run simulation server. /// \param[in] _sdfString SDF file to run, as a string. @@ -62,8 +62,8 @@ extern "C" GZ_SIM_VISIBLE const char *worldInstallDir(); /// \param[in] _recordPeriod --record-period option /// \param[in] _seed --seed value to be used for random number generator. /// \return 0 if successful, 1 if not. -extern "C" GZ_SIM_VISIBLE int runServer(const char *_sdfString, - int _iterations, int _run, float _hz, int _levels, +extern "C" GZ_SIM_GZ_VISIBLE int runServer(const char *_sdfString, + int _iterations, int _run, float _hz, double _initialSimTime, int _levels, const char *_networkRole, int _networkSecondaries, int _record, const char *_recordPath, int _recordResources, int _logOverwrite, int _logCompress, const char *_playback, @@ -82,16 +82,17 @@ extern "C" GZ_SIM_VISIBLE int runServer(const char *_sdfString, /// \param[in] _renderEngine --render-engine-gui option /// \param[in] _renderEngineGuiApiBackend --render-engine-gui-api-backend option /// \return 0 if successful, 1 if not. -extern "C" int GZ_SIM_VISIBLE runGui(const char *_guiConfig, const char *_file, - int _waitGui, - const char *_renderEngine, - const char *_renderEngineGuiApiBackend); +extern "C" GZ_SIM_GZ_VISIBLE +int runGui(const char *_guiConfig, const char *_file, + int _waitGui, + const char *_renderEngine, + const char *_renderEngineGuiApiBackend); /// \brief External hook to find or download a fuel world provided a URL. /// \param[in] _pathToResource Path to the fuel world resource, ie, /// https://staging-fuel.gazebosim.org/1.0/gmas/worlds/ShapesClone /// \return C-string containing the path to the local world sdf file -extern "C" GZ_SIM_VISIBLE const char *findFuelResource( +extern "C" GZ_SIM_GZ_VISIBLE const char *findFuelResource( char *_pathToResource); #endif diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 7487212603..f01173aba8 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -23,6 +23,8 @@ #include #include #include +#include +#include #include #include "test_config.hh" // NOLINT(build/include) @@ -140,6 +142,46 @@ TEST(CmdLine, GazeboServer) } } +///////////////////////////////////////////////// +TEST(CmdLine, SimtimeArgument) +{ + std::string cmd = + kGzCommand + " -r -v 4 --iterations 500 --initial-sim-time 1000.5 " + + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/plugins.sdf -z 1000"; + + std::cout << "Running command [" << cmd << "]" << std::endl; + int msgCount = 0; + + gz::transport::Node node; + auto cb = [&](const gz::msgs::Clock &_msg) -> void + { + EXPECT_GE(_msg.sim().sec() + 1e-9 * _msg.sim().nsec(), + 1000.5); + msgCount++; + }; + + auto cbFcn = std::function(cb); + EXPECT_TRUE(node.Subscribe(std::string("/clock"), cbFcn)); + + std::string output = customExecStr(cmd); + + // Try waiting different amounts if no messages have been received + if (!msgCount) + { + for (auto i : {1, 10, 100, 1000}) + { + std::cout << "Sleeping for " << i << " ms"; + GZ_SLEEP_MS(i); + std::cout << ", recevied " << msgCount << " messages." << std::endl; + if (msgCount) + { + break; + } + } + } + EXPECT_GT(msgCount, 0) << output; +} + ///////////////////////////////////////////////// TEST(CmdLine, Gazebo) { diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 9f8e559875..6c216ce5cf 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -637,3 +637,11 @@ bool MarkerManagerPrivate::OnMarkerMsgArray( _res.set_data(true); return true; } + +///////////////////////////////////////////////// +void MarkerManager::Clear() +{ + this->dataPtr->visuals.clear(); + this->dataPtr->markerMsgs.clear(); + this->dataPtr->scene.reset(); +} diff --git a/src/rendering/RenderUtil.cc b/src/rendering/RenderUtil.cc index 2a1de4fe45..49ed8835f5 100644 --- a/src/rendering/RenderUtil.cc +++ b/src/rendering/RenderUtil.cc @@ -76,11 +76,13 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParentLinkName.hh" #include "gz/sim/components/ParticleEmitter.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/RgbdCamera.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/SegmentationCamera.hh" #include "gz/sim/components/SemanticLabel.hh" +#include "gz/sim/components/Sensor.hh" #include "gz/sim/components/SourceFilePath.hh" #include "gz/sim/components/SphericalCoordinates.hh" #include "gz/sim/components/Temperature.hh" @@ -295,6 +297,14 @@ class gz::sim::RenderUtilPrivate public: std::unordered_map newParticleEmittersCmds; + /// \brief New projector to be created. The elements in the tuple are: + /// [0] entity id, [1] projector, [2] parent entity id + public: std::vector> + newProjectors; + + /// \brief New sensor topics that should be added to ECM as new components + public: std::unordered_map newSensorTopics; + /// \brief A list of entities with particle emitter cmds to remove public: std::vector particleCmdsToRemove; @@ -360,6 +370,11 @@ class gz::sim::RenderUtilPrivate /// \brief A map of entity ids and actor animation info. public: std::unordered_map actorAnimationData; + /// \brief A map of entity ids and the world pose of actor at current + /// timestep. The pose data is used to update the WorldPose component in the + /// ECM + public: std::unordered_map actorWorldPoses; + /// \brief True to update skeletons manually using bone poses /// (see actorTransforms). False to let render engine update animation /// based on sim time. @@ -766,6 +781,35 @@ void RenderUtil::UpdateECM(const UpdateInfo &/*_info*/, _ecm.RemoveComponent(entity); } } + + // create sensor topic components + { + for (const auto &it : this->dataPtr->newSensorTopics) + { + // Set topic + _ecm.CreateComponent(it.first, components::SensorTopic(it.second)); + } + this->dataPtr->newSensorTopics.clear(); + } + + + // update actor world pose + { + for (const auto &it : this->dataPtr->actorWorldPoses) + { + // set world pose + auto worldPoseComp = _ecm.Component(it.first); + if (!worldPoseComp) + { + _ecm.CreateComponent(it.first, components::WorldPose(it.second)); + } + else + { + worldPoseComp->Data() = it.second; + } + } + this->dataPtr->actorWorldPoses.clear(); + } } ////////////////////////////////////////////////// @@ -1062,6 +1106,7 @@ void RenderUtil::Update() auto newParticleEmitters = std::move(this->dataPtr->newParticleEmitters); auto newParticleEmittersCmds = std::move(this->dataPtr->newParticleEmittersCmds); + auto newProjectors = std::move(this->dataPtr->newProjectors); auto removeEntities = std::move(this->dataPtr->removeEntities); auto entityPoses = std::move(this->dataPtr->entityPoses); auto entityLights = std::move(this->dataPtr->entityLights); @@ -1091,6 +1136,7 @@ void RenderUtil::Update() this->dataPtr->newLights.clear(); this->dataPtr->newParticleEmitters.clear(); this->dataPtr->newParticleEmittersCmds.clear(); + this->dataPtr->newProjectors.clear(); this->dataPtr->removeEntities.clear(); this->dataPtr->entityPoses.clear(); this->dataPtr->entityLights.clear(); @@ -1185,6 +1231,13 @@ void RenderUtil::Update() entityId, std::get<1>(model), std::get<2>(model)); } + for (const auto &actor : newActors) + { + this->dataPtr->sceneManager.CreateActor( + std::get<0>(actor), std::get<1>(actor), std::get<2>(actor), + std::get<3>(actor)); + } + for (const auto &link : newLinks) { this->dataPtr->sceneManager.CreateLink( @@ -1197,13 +1250,6 @@ void RenderUtil::Update() std::get<0>(visual), std::get<1>(visual), std::get<2>(visual)); } - for (const auto &actor : newActors) - { - this->dataPtr->sceneManager.CreateActor( - std::get<0>(actor), std::get<1>(actor), std::get<2>(actor), - std::get<3>(actor)); - } - for (const auto &light : newLights) { auto newLightRendering = this->dataPtr->sceneManager.CreateLight( @@ -1241,6 +1287,13 @@ void RenderUtil::Update() emitterCmd.first, emitterCmd.second); } + for (const auto &projector : newProjectors) + { + this->dataPtr->sceneManager.CreateProjector( + std::get<0>(projector), std::get<1>(projector), + std::get<2>(projector)); + } + if (this->dataPtr->enableSensors && this->dataPtr->createSensorCb) { for (const auto &sensor : newSensors) @@ -1317,7 +1370,7 @@ void RenderUtil::Update() { auto actorMesh = this->dataPtr->sceneManager.ActorMeshById(tf.first); auto actorVisual = this->dataPtr->sceneManager.NodeById(tf.first); - if (!actorMesh || !actorVisual) + if (!actorVisual) { gzerr << "Actor with Entity ID '" << tf.first << "'. not found. " << "Skipping skeleton animation update." << std::endl; @@ -1343,10 +1396,17 @@ void RenderUtil::Update() trajPose.Rot() = tf.second["actorPose"].Rotation(); } - actorVisual->SetLocalPose(globalPose * trajPose); + math::Pose3d worldPose = globalPose * trajPose; + actorVisual->SetLocalPose(worldPose); + { + // populate world pose map which is used to update ECM + std::lock_guard lock(this->dataPtr->updateMutex); + this->dataPtr->actorWorldPoses[tf.first] = worldPose; + } tf.second.erase("actorPose"); - actorMesh->SetSkeletonLocalTransforms(tf.second); + if (actorMesh) + actorMesh->SetSkeletonLocalTransforms(tf.second); } } else @@ -1648,6 +1708,7 @@ void RenderUtilPrivate::AddNewSensor(const EntityComponentManager &_ecm, { sdfDataCopy.SetTopic(scopedName(_entity, _ecm) + _topicSuffix); } + this->newSensorTopics[_entity] = sdfDataCopy.Topic(); this->newSensors.push_back( std::make_tuple(_entity, std::move(sdfDataCopy), _parent)); this->sensorEntities.insert(_entity); @@ -1838,6 +1899,17 @@ void RenderUtilPrivate::CreateEntitiesFirstUpdate( return true; }); + // projectors + _ecm.Each( + [&](const Entity &_entity, + const components::Projector *_projector, + const components::ParentEntity *_parent) -> bool + { + this->newProjectors.push_back( + std::make_tuple(_entity, _projector->Data(), _parent->Data())); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -2116,6 +2188,17 @@ void RenderUtilPrivate::CreateEntitiesRuntime( return true; }); + // projectors + _ecm.EachNew( + [&](const Entity &_entity, + const components::Projector *_projector, + const components::ParentEntity *_parent) -> bool + { + this->newProjectors.push_back( + std::make_tuple(_entity, _projector->Data(), _parent->Data())); + return true; + }); + if (this->enableSensors) { // Create cameras @@ -2479,6 +2562,14 @@ void RenderUtilPrivate::RemoveRenderingEntities( return true; }); + // projectors + _ecm.EachRemoved( + [&](const Entity &_entity, const components::Projector *)->bool + { + this->removeEntities[_entity] = _info.iterations; + return true; + }); + // cameras _ecm.EachRemoved( [&](const Entity &_entity, const components::Camera *)->bool @@ -2642,6 +2733,21 @@ void RenderUtil::Init() this->dataPtr->markerManager.Init(this->dataPtr->scene); } +///////////////////////////////////////////////// +void RenderUtil::Destroy() +{ + if (!this->dataPtr->engine || !this->dataPtr->scene) + return; + this->dataPtr->wireBoxes.clear(); + this->dataPtr->sceneManager.Clear(); + this->dataPtr->markerManager.Clear(); + this->dataPtr->engine->DestroyScene(this->dataPtr->scene); + this->dataPtr->scene.reset(); + rendering::unloadEngine(this->dataPtr->engine->Name()); + this->dataPtr->engine = nullptr; + this->dataPtr->initialized = false; +} + ///////////////////////////////////////////////// void RenderUtil::SetBackgroundColor(const math::Color &_color) { @@ -3123,7 +3229,7 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_mapsceneManager.NodeById(it.first); auto actorSkel = this->sceneManager.ActorSkeletonById( it.first); - if (!actorMesh || !actorVisual || !actorSkel) + if (!actorVisual) { gzerr << "Actor with Entity ID '" << it.first << "'. not found. " << "Skipping skeleton animation update." << std::endl; @@ -3136,51 +3242,55 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_mapSkeletonAnimationEnabled(animData.animationName)) + + if (actorMesh && actorSkel) { - // disable all animations for this actor - for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i) + // Enable skeleton animation + if (!actorMesh->SkeletonAnimationEnabled(animData.animationName)) { + // disable all animations for this actor + for (unsigned int i = 0; i < actorSkel->AnimationCount(); ++i) + { + actorMesh->SetSkeletonAnimationEnabled( + actorSkel->Animation(i)->Name(), false, false, 0.0); + } + + // enable requested animation actorMesh->SetSkeletonAnimationEnabled( - actorSkel->Animation(i)->Name(), false, false, 0.0); + animData.animationName, true, animData.loop); + + // Set skeleton root node weight to zero so it is not affected by + // the animation being played. This is needed if trajectory animation + // is enabled. We need to let the trajectory animation set the + // position of the actor instead + common::SkeletonPtr skeleton = + this->sceneManager.ActorSkeletonById(it.first); + if (skeleton) + { + float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0; + std::unordered_map weights; + weights[skeleton->RootNode()->Name()] = rootBoneWeight; + actorMesh->SetSkeletonWeights(weights); + } } - - // enable requested animation - actorMesh->SetSkeletonAnimationEnabled( - animData.animationName, true, animData.loop); - - // Set skeleton root node weight to zero so it is not affected by - // the animation being played. This is needed if trajectory animation - // is enabled. We need to let the trajectory animation set the - // position of the actor instead - common::SkeletonPtr skeleton = - this->sceneManager.ActorSkeletonById(it.first); - if (skeleton) - { - float rootBoneWeight = (animData.followTrajectory) ? 0.0 : 1.0; - std::unordered_map weights; - weights[skeleton->RootNode()->Name()] = rootBoneWeight; - actorMesh->SetSkeletonWeights(weights); + // Update skeleton animation by setting animation time. + // Note that animation time is different from sim time. An actor can + // have multiple animations. Animation time is associated with + // current animation that is being played. It is also adjusted if + // interpotate_x is enabled. + actorMesh->UpdateSkeletonAnimation(animData.time); + + // manually update root transform in order to sync with trajectory + // animation + if (animData.followTrajectory) + { + common::SkeletonPtr skeleton = + this->sceneManager.ActorSkeletonById(it.first); + std::map rootTf; + rootTf[skeleton->RootNode()->Name()] = animData.rootTransform; + actorMesh->SetSkeletonLocalTransforms(rootTf); } } - // Update skeleton animation by setting animation time. - // Note that animation time is different from sim time. An actor can - // have multiple animations. Animation time is associated with - // current animation that is being played. It is also adjusted if - // interpotate_x is enabled. - actorMesh->UpdateSkeletonAnimation(animData.time); - - // manually update root transform in order to sync with trajectory - // animation - if (animData.followTrajectory) - { - common::SkeletonPtr skeleton = - this->sceneManager.ActorSkeletonById(it.first); - std::map rootTf; - rootTf[skeleton->RootNode()->Name()] = animData.rootTransform; - actorMesh->SetSkeletonLocalTransforms(rootTf); - } // update actor trajectory animation math::Pose3d globalPose; @@ -3207,7 +3317,12 @@ void RenderUtilPrivate::UpdateAnimation(const std::unordered_mapSetLocalPose(globalPose * trajPose); + math::Pose3d worldPose = globalPose * trajPose; + actorVisual->SetLocalPose(worldPose); + + // populate world pose map which is used to update ECM + std::lock_guard lock(this->updateMutex); + this->actorWorldPoses[it.first] = worldPose; } } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index 07e24c07fa..55d10e9534 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -67,6 +67,7 @@ #include #include #include +#include #include #include #include @@ -110,7 +111,12 @@ class gz::sim::SceneManagerPrivate /// \brief Map of particle emitter entity in Gazebo to particle emitter /// rendering pointers. - public: std::map particleEmitters; + public: std::unordered_map + particleEmitters; + + /// \brief Map of projector entity in Gazebo to projector + /// rendering pointers. + public: std::unordered_map projectors; /// \brief Map of sensor entity in Gazebo to sensor pointers. public: std::unordered_map sensors; @@ -1022,29 +1028,36 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, descriptor.meshName = asFullPath(_actor.SkinFilename(), _actor.FilePath()); common::MeshManager *meshManager = common::MeshManager::Instance(); descriptor.mesh = meshManager->Load(descriptor.meshName); + std::unordered_map mapAnimNameId; + common::SkeletonPtr meshSkel; if (nullptr == descriptor.mesh) { - gzerr << "Actor skin mesh [" << descriptor.meshName << "] not found." + gzwarn << "Actor skin mesh [" << descriptor.meshName << "] not found." << std::endl; - return rendering::VisualPtr(); } - - // todo(anyone) create a copy of meshSkel so we don't modify the original - // when adding animations! - common::SkeletonPtr meshSkel = descriptor.mesh->MeshSkeleton(); - if (nullptr == meshSkel) + else { - gzerr << "Mesh skeleton in [" << descriptor.meshName << "] not found." - << std::endl; - return rendering::VisualPtr(); + // todo(anyone) create a copy of meshSkel so we don't modify the original + // when adding animations! + meshSkel = descriptor.mesh->MeshSkeleton(); + if (nullptr == meshSkel) + { + gzwarn << "Mesh skeleton in [" << descriptor.meshName << "] not found." + << std::endl; + } + else + { + this->dataPtr->actorSkeletons[_id] = meshSkel; + // Load all animations + mapAnimNameId = this->LoadAnimations(_actor); + if (mapAnimNameId.size() == 0) + return nullptr; + } } - // Load all animations - auto mapAnimNameId = this->LoadAnimations(_actor); - if (mapAnimNameId.size() == 0) - return nullptr; - - this->dataPtr->actorSkeletons[_id] = meshSkel; + // load trajectories regardless of whether the mesh skeleton exists + // or not. If there is no mesh skeleon, we can still do trajectory + // animation auto trajectories = this->dataPtr->LoadTrajectories(_actor, mapAnimNameId, meshSkel); @@ -1069,26 +1082,28 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id, this->dataPtr->actorTrajectories[_id] = trajectories; + rendering::VisualPtr actorVisual = this->dataPtr->scene->CreateVisual(name); + rendering::MeshPtr actorMesh; // create mesh with animations - rendering::MeshPtr actorMesh = this->dataPtr->scene->CreateMesh( - descriptor); - if (nullptr == actorMesh) + if (descriptor.mesh) { - gzerr << "Actor skin file [" << descriptor.meshName << "] not found." - << std::endl; - return rendering::VisualPtr(); + actorMesh = this->dataPtr->scene->CreateMesh(descriptor); + if (nullptr == actorMesh) + { + gzerr << "Actor skin file [" << descriptor.meshName << "] not found." + << std::endl; + return rendering::VisualPtr(); + } + actorVisual->AddGeometry(actorMesh); } - rendering::VisualPtr actorVisual = this->dataPtr->scene->CreateVisual(name); actorVisual->SetLocalPose(_actor.RawPose()); - actorVisual->AddGeometry(actorMesh); actorVisual->SetUserData("gazebo-entity", _id); actorVisual->SetUserData("pause-update", static_cast(0)); this->dataPtr->visuals[_id] = actorVisual; this->dataPtr->actors[_id] = actorMesh; - if (parent) parent->AddChild(actorVisual); else @@ -1657,6 +1672,65 @@ rendering::ParticleEmitterPtr SceneManager::UpdateParticleEmitter(Entity _id, return emitter; } +///////////////////////////////////////////////// +rendering::ProjectorPtr SceneManager::CreateProjector( + Entity _id, const sdf::Projector &_projector, Entity _parentId) +{ + if (!this->dataPtr->scene) + return rendering::ProjectorPtr(); + + if (this->dataPtr->projectors.find(_id) != + this->dataPtr->projectors.end()) + { + gzerr << "Projector with Id: [" << _id << "] already exists in the " + << "scene" << std::endl; + return rendering::ProjectorPtr(); + } + + rendering::VisualPtr parent; + if (_parentId != this->dataPtr->worldId) + { + auto it = this->dataPtr->visuals.find(_parentId); + if (it == this->dataPtr->visuals.end()) + { + // It is possible to get here if the model entity is created then + // removed in between render updates. + return rendering::ProjectorPtr(); + } + parent = it->second; + } + + // Name. + std::string name = _projector.Name().empty() ? std::to_string(_id) : + _projector.Name(); + if (parent) + name = parent->Name() + "::" + name; + + rendering::ProjectorPtr projector; + projector = std::dynamic_pointer_cast( + this->dataPtr->scene->Extension()->CreateExt("projector", name)); + // \todo(iche033) replace above call with CreateProjector in gz-rendering8 + // projector = this->dataPtr->scene->CreateProjector(name); + + this->dataPtr->projectors[_id] = projector; + + if (parent) + parent->AddChild(projector); + + projector->SetLocalPose(_projector.RawPose()); + projector->SetNearClipPlane(_projector.NearClip()); + projector->SetFarClipPlane(_projector.FarClip()); + std::string texture = _projector.Texture(); + std::string fullPath = common::findFile( + asFullPath(texture, _projector.FilePath())); + + projector->SetTexture(fullPath); + projector->SetHFOV(_projector.HorizontalFov()); + projector->SetVisibilityFlags(_projector.VisibilityFlags()); + + return projector; +} + ///////////////////////////////////////////////// bool SceneManager::AddSensor(Entity _gazeboId, const std::string &_sensorName, Entity _parentGazeboId) @@ -1711,6 +1785,8 @@ bool SceneManager::HasEntity(Entity _id) const this->dataPtr->lights.find(_id) != this->dataPtr->lights.end() || this->dataPtr->particleEmitters.find(_id) != this->dataPtr->particleEmitters.end() || + this->dataPtr->projectors.find(_id) != + this->dataPtr->projectors.end() || this->dataPtr->sensors.find(_id) != this->dataPtr->sensors.end(); } @@ -1737,6 +1813,11 @@ rendering::NodePtr SceneManager::NodeById(Entity _id) const { return pIt->second; } + auto prIt = this->dataPtr->projectors.find(_id); + if (prIt != this->dataPtr->projectors.end()) + { + return prIt->second; + } return rendering::NodePtr(); } @@ -1771,12 +1852,12 @@ AnimationUpdateData SceneManager::ActorAnimationAt( if (trajIt == this->dataPtr->actorTrajectories.end()) return animData; + animData = this->dataPtr->ActorTrajectoryAt(_id, _time); + auto skelIt = this->dataPtr->actorSkeletons.find(_id); if (skelIt == this->dataPtr->actorSkeletons.end()) return animData; - animData = this->dataPtr->ActorTrajectoryAt(_id, _time); - auto skel = skelIt->second; unsigned int animIndex = animData.trajectory.AnimIndex(); animData.animationName = skel->Animation(animIndex)->Name(); @@ -1997,6 +2078,16 @@ void SceneManager::RemoveEntity(Entity _id) } } + { + auto it = this->dataPtr->projectors.find(_id); + if (it != this->dataPtr->projectors.end()) + { + this->dataPtr->scene->DestroyVisual(it->second); + this->dataPtr->projectors.erase(it); + return; + } + } + { auto it = this->dataPtr->sensors.find(_id); if (it != this->dataPtr->sensors.end()) @@ -2449,11 +2540,27 @@ SceneManagerPrivate::LoadTrajectories(const sdf::Actor &_actor, trajInfo.SetAnimIndex(0); trajInfo.SetStartTime(TP(0ms)); + double animLength = (_meshSkel) ? _meshSkel->Animation(0)->Length() : 0.0; auto timepoint = std::chrono::milliseconds( - static_cast(_meshSkel->Animation(0)->Length() * 1000)); + static_cast(animLength * 1000)); trajInfo.SetEndTime(TP(timepoint)); trajInfo.SetTranslated(false); trajectories.push_back(trajInfo); } return trajectories; } + +///////////////////////////////////////////////// +void SceneManager::Clear() +{ + this->dataPtr->visuals.clear(); + this->dataPtr->actors.clear(); + this->dataPtr->actorSkeletons.clear(); + this->dataPtr->actorTrajectories.clear(); + this->dataPtr->lights.clear(); + this->dataPtr->particleEmitters.clear(); + this->dataPtr->sensors.clear(); + this->dataPtr->scene.reset(); + this->dataPtr->originalTransparency.clear(); + this->dataPtr->originalDepthWrite.clear(); +} diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index 8eb24be389..ad19da6079 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -85,8 +85,8 @@ function(gz_add_system system_name) if(WIN32) # symlinks on Windows require admin priviledges, fallback to copy INSTALL(CODE "EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E copy - ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}\/${versioned} - ${IGNITION_GAZEBO_PLUGIN_INSTALL_DIR}\/${unversioned})") + ${GZ_SIM_PLUGIN_INSTALL_DIR}\/${versioned} + ${GZ_SIM_PLUGIN_INSTALL_DIR}\/${unversioned})") else() file(MAKE_DIRECTORY "${PROJECT_BINARY_DIR}/lib") EXECUTE_PROCESS(COMMAND ${CMAKE_COMMAND} -E create_symlink ${versioned} ${unversioned} WORKING_DIRECTORY "${PROJECT_BINARY_DIR}/lib") @@ -97,6 +97,7 @@ endfunction() add_subdirectory(ackermann_steering) add_subdirectory(acoustic_comms) add_subdirectory(air_pressure) +add_subdirectory(air_speed) add_subdirectory(altimeter) add_subdirectory(apply_joint_force) add_subdirectory(apply_link_wrench) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 7a573991d4..19b40e1962 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -17,6 +18,7 @@ #include "AckermannSteering.hh" +#include #include #include @@ -31,6 +33,7 @@ #include #include +#include "gz/sim/components/Actuators.hh" #include "gz/sim/components/CanonicalLink.hh" #include "gz/sim/components/JointPosition.hh" #include "gz/sim/components/JointVelocityCmd.hh" @@ -60,6 +63,14 @@ class gz::sim::systems::AckermannSteeringPrivate /// \param[in] _msg Velocity message public: void OnCmdVel(const msgs::Twist &_msg); + /// \brief Callback for angle subscription + /// \param[in] _msg angle message + public: void OnCmdAng(const msgs::Double &_msg); + + /// \brief Callback for actuator angle subscription + /// \param[in] _msg angle message + public: void OnActuatorAng(const msgs::Actuators &_msg); + /// \brief Update odometry and publish an odometry message. /// \param[in] _info System update information. /// \param[in] _ecm The EntityComponentManager of the given simulation @@ -74,9 +85,19 @@ class gz::sim::systems::AckermannSteeringPrivate public: void UpdateVelocity(const UpdateInfo &_info, const EntityComponentManager &_ecm); + /// \brief Update the angle. + /// \param[in] _info System update information. + /// \param[in] _ecm The EntityComponentManager of the given simulation + /// instance. + public: void UpdateAngle(const UpdateInfo &_info, + const EntityComponentManager &_ecm); + /// \brief Gazebo communication node. public: transport::Node node; + /// \brief Use angle steer only mode. + public: bool steeringOnly{false}; + /// \brief Entity of the left joint public: std::vector leftJoints; @@ -128,12 +149,18 @@ class gz::sim::systems::AckermannSteeringPrivate /// \brief Wheel radius public: double wheelRadius{0.2}; + /// \brief Index of angle actuator. + public: int actuatorNumber = 0; + /// \brief Model interface public: Model model{kNullEntity}; /// \brief The model's canonical link. public: Link canonicalLink{kNullEntity}; + /// \brief True if using Actuator msg to control steering angle. + public: bool useActuatorMsg{false}; + /// \brief Update period calculated from . public: std::chrono::steady_clock::duration odomPubPeriod{0}; @@ -179,6 +206,12 @@ class gz::sim::systems::AckermannSteeringPrivate /// \brief Last target velocity requested. public: msgs::Twist targetVel; + /// \brief Last target angle requested. + public: double targetAng{0.0}; + + /// \brief P gain for angular position. + public: double gainPAng{1.0}; + /// \brief A mutex to protect the target velocity command. public: std::mutex mutex; @@ -216,48 +249,87 @@ void AckermannSteering::Configure(const Entity &_entity, if (!links.empty()) this->dataPtr->canonicalLink = Link(links[0]); - // Ugly, but needed because the sdf::Element::GetElement is not a const - // function and _sdf is a const shared pointer to a const sdf::Element. - auto ptr = const_cast(_sdf.get()); - - // Get params from SDF - sdf::ElementPtr sdfElem = ptr->GetElement("left_joint"); - while (sdfElem) + if (_sdf->HasElement("steering_only")) { - this->dataPtr->leftJointNames.push_back(sdfElem->Get()); - sdfElem = sdfElem->GetNextElement("left_joint"); + this->dataPtr->steeringOnly = _sdf->Get("steering_only"); + gzmsg << "Using steering only mode: " << this->dataPtr->steeringOnly + << std::endl; } - sdfElem = ptr->GetElement("right_joint"); - while (sdfElem) + + if (_sdf->HasElement("use_actuator_msg") && + _sdf->Get("use_actuator_msg")) { - this->dataPtr->rightJointNames.push_back(sdfElem->Get()); - sdfElem = sdfElem->GetNextElement("right_joint"); + if (_sdf->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + _sdf->Get("actuator_number"); + this->dataPtr->useActuatorMsg = true; + if (!this->dataPtr->steeringOnly) + { + this->dataPtr->steeringOnly = true; + } + } + else + { + gzerr << "Please specify an actuator_number" << + "to use Actuator position message control." << std::endl; + } } - sdfElem = ptr->GetElement("left_steering_joint"); - while (sdfElem) + + // Get params from SDF + auto sdfLeftSteerElem = _sdf->FindElement("left_steering_joint"); + while (sdfLeftSteerElem) { this->dataPtr->leftSteeringJointNames.push_back( - sdfElem->Get()); - sdfElem = sdfElem->GetNextElement("left_steering_joint"); + sdfLeftSteerElem->Get()); + sdfLeftSteerElem = sdfLeftSteerElem->GetNextElement( + "left_steering_joint"); } - sdfElem = ptr->GetElement("right_steering_joint"); - while (sdfElem) + auto sdfRightSteerElem = _sdf->FindElement("right_steering_joint"); + while (sdfRightSteerElem) { this->dataPtr->rightSteeringJointNames.push_back( - sdfElem->Get()); - sdfElem = sdfElem->GetNextElement("right_steering_joint"); + sdfRightSteerElem->Get()); + sdfRightSteerElem = sdfRightSteerElem->GetNextElement( + "right_steering_joint"); + } + if (!this->dataPtr->steeringOnly) + { + auto sdfLeftElem = _sdf->FindElement("left_joint"); + while (sdfLeftElem) + { + this->dataPtr->leftJointNames.push_back( + sdfLeftElem->Get()); + sdfLeftElem = sdfLeftElem->GetNextElement("left_joint"); + } + auto sdfRightElem = _sdf->FindElement("right_joint"); + while (sdfRightElem) + { + this->dataPtr->rightJointNames.push_back( + sdfRightElem->Get()); + sdfRightElem = sdfRightElem->GetNextElement("right_joint"); + } + } + if (!this->dataPtr->steeringOnly) + { + this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", + this->dataPtr->wheelRadius).first; + this->dataPtr->kingpinWidth = _sdf->Get("kingpin_width", + this->dataPtr->kingpinWidth).first; } - this->dataPtr->wheelSeparation = _sdf->Get("wheel_separation", this->dataPtr->wheelSeparation).first; - this->dataPtr->kingpinWidth = _sdf->Get("kingpin_width", - this->dataPtr->kingpinWidth).first; + this->dataPtr->wheelBase = _sdf->Get("wheel_base", this->dataPtr->wheelBase).first; this->dataPtr->steeringLimit = _sdf->Get("steering_limit", this->dataPtr->steeringLimit).first; - this->dataPtr->wheelRadius = _sdf->Get("wheel_radius", - this->dataPtr->wheelRadius).first; + + // Get proportional gain for steering angle. + if (_sdf->HasElement("steer_p_gain")) + { + this->dataPtr->gainPAng = _sdf->Get("steer_p_gain"); + } // Instantiate the speed limiters. this->dataPtr->limiterLin = std::make_unique(); @@ -301,77 +373,123 @@ void AckermannSteering::Configure(const Entity &_entity, this->dataPtr->limiterAng->SetMaxJerk(maxJerk); } - - double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; - if (odomFreq > 0) + if (!this->dataPtr->steeringOnly) { - std::chrono::duration odomPer{1 / odomFreq}; - this->dataPtr->odomPubPeriod = - std::chrono::duration_cast(odomPer); + double odomFreq = _sdf->Get("odom_publish_frequency", 50).first; + if (odomFreq > 0) + { + std::chrono::duration odomPer{1 / odomFreq}; + this->dataPtr->odomPubPeriod = + std::chrono::duration_cast( + odomPer); + } } - // Subscribe to commands std::vector topics; + + if (_sdf->HasElement("topic")) { topics.push_back(_sdf->Get("topic")); } - topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); - auto topic = validTopic(topics); - if (topic.empty()) + else if (_sdf->HasElement("sub_topic")) { - gzerr << "AckermannSteering plugin received invalid model name " - << "Failed to initialize." << std::endl; - return; + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/" + _sdf->Get("sub_topic")); } - - this->dataPtr->node.Subscribe(topic, &AckermannSteeringPrivate::OnCmdVel, - this->dataPtr.get()); - - std::vector odomTopics; - if (_sdf->HasElement("odom_topic")) + else if ((this->dataPtr->steeringOnly) && + (!this->dataPtr->useActuatorMsg)) + { + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/steer_angle"); + } + else if ((this->dataPtr->steeringOnly) && + (this->dataPtr->useActuatorMsg)) { - odomTopics.push_back(_sdf->Get("odom_topic")); + topics.push_back("/actuators"); } - odomTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + - "/odometry"); - auto odomTopic = validTopic(odomTopics); + else if (!this->dataPtr->steeringOnly) + { + topics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + "/cmd_vel"); + } + + auto topic = validTopic(topics); if (topic.empty()) { - gzerr << "AckermannSteering plugin received invalid model name " + gzerr << "AckermannSteering plugin received invalid topic name " << "Failed to initialize." << std::endl; return; } - - this->dataPtr->odomPub = this->dataPtr->node.Advertise( - odomTopic); - - std::vector tfTopics; - if (_sdf->HasElement("tf_topic")) + if (this->dataPtr->steeringOnly) { - tfTopics.push_back(_sdf->Get("tf_topic")); + if (this->dataPtr->useActuatorMsg) + { + this->dataPtr->node.Subscribe(topic, + &AckermannSteeringPrivate::OnActuatorAng, + this->dataPtr.get()); + gzmsg << "AckermannSteering subscribing to Actuator messages on [" + << topic << "]" << std::endl; + } + else + { + this->dataPtr->node.Subscribe(topic, + &AckermannSteeringPrivate::OnCmdAng, + this->dataPtr.get()); + gzmsg << "AckermannSteering subscribing to float messages on [" + << topic << "]" << std::endl; + } } - tfTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + - "/tf"); - auto tfTopic = validTopic(tfTopics); - if (tfTopic.empty()) + else { - gzerr << "AckermannSteering plugin invalid tf topic name " - << "Failed to initialize." << std::endl; - return; + this->dataPtr->node.Subscribe(topic, &AckermannSteeringPrivate::OnCmdVel, + this->dataPtr.get()); + gzmsg << "AckermannSteering subscribing to twist messages on [" + << topic << "]" << std::endl; } + if (!this->dataPtr->steeringOnly) + { + std::vector odomTopics; + if (_sdf->HasElement("odom_topic")) + { + odomTopics.push_back(_sdf->Get("odom_topic")); + } + odomTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/odometry"); + auto odomTopic = validTopic(odomTopics); + if (topic.empty()) + { + gzerr << "AckermannSteering plugin received invalid model name " + << "Failed to initialize." << std::endl; + return; + } + + this->dataPtr->odomPub = this->dataPtr->node.Advertise( + odomTopic); - this->dataPtr->tfPub = this->dataPtr->node.Advertise( - tfTopic); + std::vector tfTopics; + if (_sdf->HasElement("tf_topic")) + { + tfTopics.push_back(_sdf->Get("tf_topic")); + } + tfTopics.push_back("/model/" + this->dataPtr->model.Name(_ecm) + + "/tf"); + auto tfTopic = validTopic(tfTopics); + if (tfTopic.empty()) + { + gzerr << "AckermannSteering plugin invalid tf topic name " + << "Failed to initialize." << std::endl; + return; + } - if (_sdf->HasElement("frame_id")) - this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); + this->dataPtr->tfPub = this->dataPtr->node.Advertise( + tfTopic); - if (_sdf->HasElement("child_frame_id")) - this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + if (_sdf->HasElement("frame_id")) + this->dataPtr->sdfFrameId = _sdf->Get("frame_id"); - gzmsg << "AckermannSteering subscribing to twist messages on [" << - topic << "]" << std::endl; + if (_sdf->HasElement("child_frame_id")) + this->dataPtr->sdfChildFrameId = _sdf->Get("child_frame_id"); + } } ////////////////////////////////////////////////// @@ -391,10 +509,9 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, // If the joints haven't been identified yet, look for them static std::set warnedModels; auto modelName = this->dataPtr->model.Name(_ecm); - if (this->dataPtr->leftJoints.empty() || - this->dataPtr->rightJoints.empty() || - this->dataPtr->leftSteeringJoints.empty() || - this->dataPtr->rightSteeringJoints.empty()) + if (!this->dataPtr->steeringOnly && + (this->dataPtr->leftJoints.empty() || + this->dataPtr->rightJoints.empty())) { bool warned{false}; for (const std::string &name : this->dataPtr->leftJointNames) @@ -422,6 +539,15 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, warned = true; } } + if (warned) + { + warnedModels.insert(modelName); + } + } + if (this->dataPtr->leftSteeringJoints.empty() || + this->dataPtr->rightSteeringJoints.empty()) + { + bool warned{false}; for (const std::string &name : this->dataPtr->leftSteeringJointNames) { Entity joint = this->dataPtr->model.JointByName(_ecm, name); @@ -453,9 +579,11 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, warnedModels.insert(modelName); } } - - if (this->dataPtr->leftJoints.empty() || this->dataPtr->rightJoints.empty() || - this->dataPtr->leftSteeringJoints.empty() || + if (!this->dataPtr->steeringOnly && + (this->dataPtr->leftJoints.empty() || + this->dataPtr->rightJoints.empty())) + return; + else if (this->dataPtr->leftSteeringJoints.empty() || this->dataPtr->rightSteeringJoints.empty()) return; @@ -469,36 +597,39 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, // Nothing left to do if paused. if (_info.paused) return; - - for (Entity joint : this->dataPtr->leftJoints) + if (!this->dataPtr->steeringOnly) { - // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) + for (Entity joint : this->dataPtr->leftJoints) { - _ecm.CreateComponent( - joint, components::JointVelocityCmd({this->dataPtr->leftJointSpeed})); - } - else - { - *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); - } - } + // Update wheel velocity + auto vel = _ecm.Component(joint); - for (Entity joint : this->dataPtr->rightJoints) - { - // Update wheel velocity - auto vel = _ecm.Component(joint); - - if (vel == nullptr) - { - _ecm.CreateComponent(joint, - components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd( + {this->dataPtr->leftJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->leftJointSpeed}); + } } - else + + for (Entity joint : this->dataPtr->rightJoints) { - *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); + // Update wheel velocity + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent(joint, + components::JointVelocityCmd({this->dataPtr->rightJointSpeed})); + } + else + { + *vel = components::JointVelocityCmd({this->dataPtr->rightJointSpeed}); + } } } @@ -536,25 +667,26 @@ void AckermannSteering::PreUpdate(const UpdateInfo &_info, {this->dataPtr->rightSteeringJointSpeed}); } } - - // Create the left and right side joint position components if they - // don't exist. - auto leftPos = _ecm.Component( - this->dataPtr->leftJoints[0]); - if (!leftPos) + if (!this->dataPtr->steeringOnly) { - _ecm.CreateComponent(this->dataPtr->leftJoints[0], - components::JointPosition()); - } + // Create the left and right side joint position components if they + // don't exist. + auto leftPos = _ecm.Component( + this->dataPtr->leftJoints[0]); + if (!leftPos) + { + _ecm.CreateComponent(this->dataPtr->leftJoints[0], + components::JointPosition()); + } - auto rightPos = _ecm.Component( - this->dataPtr->rightJoints[0]); - if (!rightPos) - { - _ecm.CreateComponent(this->dataPtr->rightJoints[0], - components::JointPosition()); + auto rightPos = _ecm.Component( + this->dataPtr->rightJoints[0]); + if (!rightPos) + { + _ecm.CreateComponent(this->dataPtr->rightJoints[0], + components::JointPosition()); + } } - auto leftSteeringPos = _ecm.Component( this->dataPtr->leftSteeringJoints[0]); if (!leftSteeringPos) @@ -580,9 +712,15 @@ void AckermannSteering::PostUpdate(const UpdateInfo &_info, // Nothing left to do if paused. if (_info.paused) return; - - this->dataPtr->UpdateVelocity(_info, _ecm); - this->dataPtr->UpdateOdometry(_info, _ecm); + if (this->dataPtr->steeringOnly) + { + this->dataPtr->UpdateAngle(_info, _ecm); + } + else + { + this->dataPtr->UpdateVelocity(_info, _ecm); + this->dataPtr->UpdateOdometry(_info, _ecm); + } } ////////////////////////////////////////////////// @@ -729,14 +867,23 @@ void AckermannSteeringPrivate::UpdateVelocity( // Convert the target velocities to joint velocities and angles double turningRadius = linVel / angVel; double minimumTurningRadius = this->wheelBase / sin(this->steeringLimit); - if ((turningRadius >= 0.0) && (turningRadius < minimumTurningRadius)) + if (fabs(linVel) > 0.0) { - turningRadius = minimumTurningRadius; + if ((turningRadius >= 0.0) && (turningRadius < minimumTurningRadius)) + { + turningRadius = minimumTurningRadius; + } + if ((turningRadius <= 0.0) && (turningRadius > -minimumTurningRadius)) + { + turningRadius = -minimumTurningRadius; + } } - if ((turningRadius <= 0.0) && (turningRadius > -minimumTurningRadius)) + // special case for angVel not zero and linVel zero + else if (fabs(angVel) >= 0.001) { - turningRadius = -minimumTurningRadius; + turningRadius = (angVel / fabs(angVel)) * minimumTurningRadius; } + // special case for angVel of zero if (fabs(angVel) < 0.001) { @@ -787,6 +934,84 @@ void AckermannSteeringPrivate::OnCmdVel(const msgs::Twist &_msg) this->targetVel = _msg; } +////////////////////////////////////////////////// +void AckermannSteeringPrivate::UpdateAngle( + const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AckermannSteering::UpdateAngle"); + + double ang; + { + std::lock_guard lock(this->mutex); + ang = this->targetAng; + } + + // Limit the target angle if needed. + this->limiterAng->Limit( + ang, this->last0Cmd.ang, this->last1Cmd.ang, _info.dt); + + if (fabs(ang) > this->steeringLimit) + { + ang = (ang / fabs(ang)) * this->steeringLimit; + } + + // Update history of commands. + this->last1Cmd = last0Cmd; + this->last0Cmd.ang = ang; + + double leftSteeringJointAngle = + atan((2.0 * this->wheelBase * sin(ang)) / \ + ((2.0 * this->wheelBase * cos(ang)) + \ + (1.0 * this->wheelSeparation * sin(ang)))); + double rightSteeringJointAngle = + atan((2.0 * this->wheelBase * sin(ang)) / \ + ((2.0 * this->wheelBase * cos(ang)) - \ + (1.0 * this->wheelSeparation * sin(ang)))); + + auto leftSteeringPos = _ecm.Component( + this->leftSteeringJoints[0]); + auto rightSteeringPos = _ecm.Component( + this->rightSteeringJoints[0]); + + // Abort if the joints were not found or just created. + if (!leftSteeringPos || !rightSteeringPos || + leftSteeringPos->Data().empty() || + rightSteeringPos->Data().empty()) + { + return; + } + + double leftDelta = leftSteeringJointAngle - leftSteeringPos->Data()[0]; + double rightDelta = rightSteeringJointAngle - rightSteeringPos->Data()[0]; + + // Simple proportional control with settable gain. + // Adding programmable PID values might be a future feature. + this->leftSteeringJointSpeed = this->gainPAng * leftDelta; + this->rightSteeringJointSpeed = this->gainPAng * rightDelta; +} + +////////////////////////////////////////////////// +void AckermannSteeringPrivate::OnCmdAng(const msgs::Double &_msg) +{ + std::lock_guard lock(this->mutex); + this->targetAng = _msg.data(); +} + +void AckermannSteeringPrivate::OnActuatorAng(const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->mutex); + if (this->actuatorNumber > _msg.position_size() - 1) + { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator position array which is of size " + << _msg.position_size() << std::endl; + return; + } + + this->targetAng = static_cast(_msg.position(this->actuatorNumber)); +} + GZ_ADD_PLUGIN(AckermannSteering, System, AckermannSteering::ISystemConfigure, diff --git a/src/systems/ackermann_steering/AckermannSteering.hh b/src/systems/ackermann_steering/AckermannSteering.hh index b0f9c75353..f82e0cec20 100644 --- a/src/systems/ackermann_steering/AckermannSteering.hh +++ b/src/systems/ackermann_steering/AckermannSteering.hh @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -37,6 +38,22 @@ namespace systems /// /// # System Parameters /// + /// ``: Boolean used to only control the steering angle + /// only. Calculates the angles of wheels from steering_limit, wheel_base, + /// and wheel_separation. Uses gz::msg::Double on default topic name + /// `/model/{name_of_model}/steer_angle`. Automatically set True when + /// `` is True, uses defaults topic name "/actuators". + /// + /// `` True to enable the use of actutor message + /// for steering angle command. Relies on `` for the + /// index of the position actuator and defaults to topic "/actuators". + /// + /// `` used with `` to set + /// the index of the steering angle position actuator. + /// + /// ``: Float used to control the steering angle P gain. + /// Only used for when in steering_only mode. + /// /// ``: Name of a joint that controls a left wheel. This /// element can appear multiple times, and must appear at least once. /// @@ -83,8 +100,13 @@ namespace systems /// ``: jerk [m/s^3], usually >= 0. /// /// ``: Custom topic that this system will subscribe to in order to - /// receive command velocity messages. This element if optional, and the - /// default value is `/model/{name_of_model}/cmd_vel`. + /// receive command messages. This element is optional, and the + /// default value is `/model/{name_of_model}/cmd_vel` or when steering_only + /// is true `/model/{name_of_model}/steer_angle`. + /// + /// ``: Custom sub_topic that this system will subscribe to in + /// order to receive command messages. This element is optional, and + /// creates a topic `/model/{name_of_model}/{sub_topic}` /// /// ``: Custom topic on which this system will publish odometry /// messages. This element if optional, and the default value is diff --git a/src/systems/air_speed/AirSpeed.cc b/src/systems/air_speed/AirSpeed.cc new file mode 100644 index 0000000000..2082e4e2a1 --- /dev/null +++ b/src/systems/air_speed/AirSpeed.cc @@ -0,0 +1,310 @@ +/* + * 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 "AirSpeed.hh" + +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include + +#include +#include + +#include "gz/sim/components/AirSpeedSensor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Private AirSpeed data class. +class gz::sim::systems::AirSpeedPrivate +{ + /// \brief A map of air speed entity to its sensor + public: std::unordered_map> entitySensorMap; + + /// \brief gz-sensors sensor factory for creating sensors + public: sensors::SensorFactory sensorFactory; + + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + + /// True if the rendering component is initialized + public: bool initialized = false; + + public: Entity entity; + + /// \brief Create sensor + /// \param[in] _ecm Immutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _airSpeed AirSpeedSensor component. + /// \param[in] _parent Parent entity component. + public: void AddAirSpeed( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::AirSpeedSensor *_airSpeed, + const components::ParentEntity *_parent); + + /// \brief Create air speed sensor + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); + + /// \brief Update air speed sensor data based on physics data + /// \param[in] _ecm Immutable reference to ECM. + public: void UpdateAirSpeeds(const EntityComponentManager &_ecm); + + /// \brief Remove air speed sensors if their entities have been removed + /// from simulation. + /// \param[in] _ecm Immutable reference to ECM. + public: void RemoveAirSpeedEntities(const EntityComponentManager &_ecm); +}; + +////////////////////////////////////////////////// +AirSpeed::AirSpeed() : + System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +AirSpeed::~AirSpeed() = default; + +////////////////////////////////////////////////// +void AirSpeed::PreUpdate(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirSpeed::PreUpdate"); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + gzerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); +} + +////////////////////////////////////////////////// +void AirSpeed::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + // Only update and publish if not paused. + GZ_PROFILE("AirSpeed::PostUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + gzwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + this->dataPtr->CreateSensors(_ecm); + + if (!_info.paused) + { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: gz-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AirSpeedPrivate::UpdateSpeeds function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + + this->dataPtr->UpdateAirSpeeds(_ecm); + + for (auto &it : this->dataPtr->entitySensorMap) + { + // Update measurement time + it.second->Update(_info.simTime, false); + } + } + + this->dataPtr->RemoveAirSpeedEntities(_ecm); +} + +////////////////////////////////////////////////// +void AirSpeedPrivate::AddAirSpeed( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::AirSpeedSensor *_airSpeed, + const components::ParentEntity *_parent) +{ + this->entity = _entity; + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _airSpeed->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/air_speed"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::AirSpeedSensor>(data); + if (nullptr == sensor) + { + gzerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + // set sensor world pose + math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); + sensor->SetPose(sensorWorldPose); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); +} + +////////////////////////////////////////////////// +void AirSpeedPrivate::CreateSensors(const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirSpeedPrivate::CreateAirSpeedEntities"); + if (!this->initialized) + { + // Create air speed sensors + _ecm.Each( + [&](const Entity &_entity, + const components::AirSpeedSensor *_airSpeed, + const components::ParentEntity *_parent)->bool + { + this->AddAirSpeed(_ecm, _entity, _airSpeed, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create air speed sensors + _ecm.EachNew( + [&](const Entity &_entity, + const components::AirSpeedSensor *_airSpeed, + const components::ParentEntity *_parent)->bool + { + this->AddAirSpeed(_ecm, _entity, _airSpeed, _parent); + return true; + }); + } +} + +////////////////////////////////////////////////// +void AirSpeedPrivate::UpdateAirSpeeds(const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirSpeedPrivate::UpdateAirSpeeds"); + _ecm.Each( + [&](const Entity &_entity, + const components::AirSpeedSensor *, + const components::WorldPose *_worldPose)->bool + { + auto it = this->entitySensorMap.find(_entity); + if (it != this->entitySensorMap.end()) + { + const math::Pose3d &worldPose = _worldPose->Data(); + it->second->SetPose(worldPose); + + math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm); + it->second->SetVelocity(sensorRelativeVel); + } + else + { + gzerr << "Failed to update air speed: " << _entity << ". " + << "Entity not found." << std::endl; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void AirSpeedPrivate::RemoveAirSpeedEntities( + const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirSpeedPrivate::RemoveAirSpeedEntities"); + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::AirSpeedSensor *)->bool + { + auto sensorId = this->entitySensorMap.find(_entity); + if (sensorId == this->entitySensorMap.end()) + { + gzerr << "Internal error, missing air speed sensor for entity [" + << _entity << "]" << std::endl; + return true; + } + + this->entitySensorMap.erase(sensorId); + + return true; + }); +} + +GZ_ADD_PLUGIN(AirSpeed, System, + AirSpeed::ISystemPreUpdate, + AirSpeed::ISystemPostUpdate +) + +GZ_ADD_PLUGIN_ALIAS(AirSpeed, "gz::sim::systems::AirSpeed") + +// TODO(CH3): Deprecated, remove on version 8 +GZ_ADD_PLUGIN_ALIAS(AirSpeed, "ignition::gazebo::systems::AirSpeed") diff --git a/src/systems/air_speed/AirSpeed.hh b/src/systems/air_speed/AirSpeed.hh new file mode 100644 index 0000000000..4c60d3b48f --- /dev/null +++ b/src/systems/air_speed/AirSpeed.hh @@ -0,0 +1,65 @@ +/* + * 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. + * +*/ +#ifndef GZ_SIM_SYSTEMS_AIRSPEED_HH_ +#define GZ_SIM_SYSTEMS_AIRSPEED_HH_ + +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class AirSpeedPrivate; + + /// \class AirSpeed AirSpeed.hh gz/sim/systems/AirSpeed.hh + /// \brief An air speed sensor that reports vertical position and velocity + /// readings over gz transport + class AirSpeed: + public System, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: explicit AirSpeed(); + + /// \brief Destructor + public: ~AirSpeed() override; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/air_speed/CMakeLists.txt b/src/systems/air_speed/CMakeLists.txt new file mode 100644 index 0000000000..1c4b15fe46 --- /dev/null +++ b/src/systems/air_speed/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(air-speed + SOURCES + AirSpeed.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + PRIVATE_LINK_LIBS + gz-sensors${GZ_SENSORS_VER}::air_speed +) diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 5f7bd1b802..06b82f98f8 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -121,7 +121,7 @@ Link decomposeMessage(const EntityComponentManager &_ecm, } gzerr << "Wrench can only be applied to a link or a model. Entity [" - << entity << "] isn't either of them." << std::endl; + << entity << "] isn't either of them." << std::endl; return Link(); } @@ -141,7 +141,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, if (!world.Valid(_ecm)) { gzerr << "ApplyLinkWrench system should be attached to a world." - << std::endl; + << std::endl; return; } @@ -157,7 +157,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, if (!elem->HasElement("entity_name") || !elem->HasElement("entity_type")) { gzerr << "Skipping element missing entity name or type." - << std::endl; + << std::endl; continue; } @@ -175,7 +175,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, else { gzerr << "Skipping element, entity type [" << typeStr - << "] not supported." << std::endl; + << "] not supported." << std::endl; continue; } @@ -202,7 +202,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, this->dataPtr.get()); gzmsg << "Listening to instantaneous wrench commands in [" << topic << "]" - << std::endl; + << std::endl; // Topic to apply wrench continuously topic = "/world/" + world.Name(_ecm).value() + "/wrench/persistent"; @@ -213,7 +213,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, &ApplyLinkWrenchPrivate::OnWrenchPersistent, this->dataPtr.get()); gzmsg << "Listening to persistent wrench commands in [" << topic << "]" - << std::endl; + << std::endl; // Topic to clear persistent wrenches topic = "/world/" + world.Name(_ecm).value() + "/wrench/clear"; @@ -224,7 +224,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, &ApplyLinkWrenchPrivate::OnWrenchClear, this->dataPtr.get()); gzmsg << "Listening to wrench clear commands in [" << topic << "]" - << std::endl; + << std::endl; } ////////////////////////////////////////////////// @@ -252,7 +252,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, if (this->dataPtr->verbose) { gzdbg << "Clearing persistent wrench for entity [" << clearEntity - << "]" << std::endl; + << "]" << std::endl; } } } @@ -275,7 +275,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, if (!link.Valid(_ecm)) { gzerr << "Entity not found." << std::endl - << msg.DebugString() << std::endl; + << msg.DebugString() << std::endl; this->dataPtr->newWrenches.pop(); continue; } @@ -285,7 +285,7 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, if (this->dataPtr->verbose) { gzdbg << "Applying wrench [" << force << " " << torque << "] to entity [" - << link.Entity() << "] for 1 time step." << std::endl; + << link.Entity() << "] for 1 time step." << std::endl; } this->dataPtr->newWrenches.pop(); @@ -315,7 +315,7 @@ void ApplyLinkWrenchPrivate::OnWrench(const msgs::EntityWrench &_msg) if (!_msg.has_entity() || !_msg.has_wrench()) { gzerr << "Missing entity or wrench in message: " << std::endl - << _msg.DebugString() << std::endl; + << _msg.DebugString() << std::endl; return; } @@ -330,14 +330,14 @@ void ApplyLinkWrenchPrivate::OnWrenchPersistent(const msgs::EntityWrench &_msg) if (!_msg.has_entity() || !_msg.has_wrench()) { gzerr << "Missing entity or wrench in message: " << std::endl - << _msg.DebugString() << std::endl; + << _msg.DebugString() << std::endl; return; } if (this->verbose) { gzdbg << "Queueing persistent wrench:" << std::endl - << _msg.DebugString() << std::endl; + << _msg.DebugString() << std::endl; } this->persistentWrenches.push_back(_msg); diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.cc b/src/systems/battery_plugin/LinearBatteryPlugin.cc index 3967c75250..dc1b0ced76 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.cc +++ b/src/systems/battery_plugin/LinearBatteryPlugin.cc @@ -39,6 +39,7 @@ #include #include +#include "gz/sim/components/BatteryPowerLoad.hh" #include "gz/sim/components/BatterySoC.hh" #include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointForceCmd.hh" @@ -78,9 +79,21 @@ class gz::sim::systems::LinearBatteryPluginPrivate const char *_data, const size_t _size, const gz::transport::MessageInfo &_info); + /// \brief Callback connected to additional topics that can stop battery + /// draining. + /// \param[in] _data Message data. + /// \param[in] _size Message data size. + /// \param[in] _info Information about the message. + public: void OnBatteryStopDrainingMsg( + const char *_data, const size_t _size, + const gz::transport::MessageInfo &_info); + /// \brief Name of model, only used for printing warning when battery drains. public: std::string modelName; + /// \brief Name that identifies a battery. + public: std::string batteryName; + /// \brief Pointer to battery contained in link. public: common::BatteryPtr battery; @@ -145,7 +158,7 @@ class gz::sim::systems::LinearBatteryPluginPrivate public: std::chrono::steady_clock::duration stepSize; /// \brief Flag on whether the battery should start draining - public: bool startDraining = true; + public: bool startDraining = false; /// \brief The start time when battery starts draining in seconds public: int drainStartTime = -1; @@ -163,8 +176,8 @@ class gz::sim::systems::LinearBatteryPluginPrivate /// \brief Battery state of charge message publisher public: transport::Node::Publisher statePub; - /// \brief Whether a topic has received any battery-draining command. - public: bool startDrainingFromTopics = false; + /// \brief Initial power load set trough config + public: double initialPowerLoad = 0.0; }; ///////////////////////////////////////////////// @@ -262,21 +275,18 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, if (_sdf->HasElement("battery_name") && _sdf->HasElement("voltage")) { - auto batteryName = _sdf->Get("battery_name"); + this->dataPtr->batteryName = _sdf->Get("battery_name"); auto initVoltage = _sdf->Get("voltage"); - // Create battery entity and component + // Create battery entity and some components this->dataPtr->batteryEntity = _ecm.CreateEntity(); - // Initialize with initial voltage - _ecm.CreateComponent(this->dataPtr->batteryEntity, - components::BatterySoC(this->dataPtr->soc)); _ecm.CreateComponent(this->dataPtr->batteryEntity, components::Name( - batteryName)); + this->dataPtr->batteryName)); _ecm.SetParentEntity(this->dataPtr->batteryEntity, _entity); // Create actual battery and assign update function - this->dataPtr->battery = std::make_shared(batteryName, - initVoltage); + this->dataPtr->battery = std::make_shared( + this->dataPtr->batteryName, initVoltage); this->dataPtr->battery->Init(); this->dataPtr->battery->SetUpdateFunc( std::bind(&LinearBatteryPlugin::OnUpdateVoltage, this, @@ -339,10 +349,10 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, // Consumer-specific if (_sdf->HasElement("power_load")) { - auto powerLoad = _sdf->Get("power_load"); + this->dataPtr->initialPowerLoad = _sdf->Get("power_load"); this->dataPtr->consumerId = this->dataPtr->battery->AddConsumer(); bool success = this->dataPtr->battery->SetPowerLoad( - this->dataPtr->consumerId, powerLoad); + this->dataPtr->consumerId, this->dataPtr->initialPowerLoad); if (!success) gzerr << "Failed to set consumer power load." << std::endl; } @@ -352,6 +362,9 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, << "in LinearBatteryPlugin SDF" << std::endl; } + if (_sdf->HasElement("start_draining")) + this->dataPtr->startDraining = _sdf->Get("start_draining"); + // Subscribe to power draining topics, if any. if (_sdf->HasElement("power_draining_topic")) { @@ -369,12 +382,33 @@ void LinearBatteryPlugin::Configure(const Entity &_entity, } } + // Subscribe to stop power draining topics, if any. + if (_sdf->HasElement("stop_power_draining_topic")) + { + sdf::ElementConstPtr sdfElem = + _sdf->FindElement("stop_power_draining_topic"); + while (sdfElem) + { + const auto &topic = sdfElem->Get(); + this->dataPtr->node.SubscribeRaw(topic, + std::bind(&LinearBatteryPluginPrivate::OnBatteryStopDrainingMsg, + this->dataPtr.get(), std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + ignmsg << "LinearBatteryPlugin subscribes to stop power draining topic [" + << topic << "]." << std::endl; + sdfElem = sdfElem->GetNextElement("power_draining_topic"); + } + } + gzmsg << "LinearBatteryPlugin configured. Battery name: " << this->dataPtr->battery->Name() << std::endl; gzdbg << "Battery initial voltage: " << this->dataPtr->battery->InitVoltage() << std::endl; this->dataPtr->soc = this->dataPtr->q / this->dataPtr->c; + // Initialize battery with initial calculated state of charge + _ecm.CreateComponent(this->dataPtr->batteryEntity, + components::BatterySoC(this->dataPtr->soc)); // Setup battery state topic std::string stateTopic{"/model/" + this->dataPtr->model.Name(_ecm) + @@ -400,7 +434,7 @@ void LinearBatteryPluginPrivate::Reset() this->iraw = 0.0; this->ismooth = 0.0; this->q = this->q0; - this->startDrainingFromTopics = false; + this->startDraining = false; } ///////////////////////////////////////////////// @@ -429,7 +463,14 @@ void LinearBatteryPluginPrivate::OnDisableRecharge( void LinearBatteryPluginPrivate::OnBatteryDrainingMsg( const char *, const size_t, const gz::transport::MessageInfo &) { - this->startDrainingFromTopics = true; + this->startDraining = true; +} + +////////////////////////////////////////////////// +void LinearBatteryPluginPrivate::OnBatteryStopDrainingMsg( + const char *, const size_t, const gz::transport::MessageInfo &) +{ + this->startDraining = false; } ////////////////////////////////////////////////// @@ -439,10 +480,26 @@ void LinearBatteryPlugin::PreUpdate( { GZ_PROFILE("LinearBatteryPlugin::PreUpdate"); - // \todo(anyone) Add in the ability to stop the battery from draining - // after it has been started by a topic. See this comment: - // https://github.com/gazebosim/gz-sim/pull/1255#discussion_r770223092 - this->dataPtr->startDraining = this->dataPtr->startDrainingFromTopics; + // Recalculate the total power load among consumers + double total_power_load = this->dataPtr->initialPowerLoad; + _ecm.Each( + [&](const Entity & /*_entity*/, + const components::BatteryPowerLoad *_batteryPowerLoadInfo)->bool + { + if (_batteryPowerLoadInfo->Data().batteryId == + this->dataPtr->batteryEntity) + { + total_power_load = total_power_load + + _batteryPowerLoadInfo->Data().batteryPowerLoad; + } + return true; + }); + + bool success = this->dataPtr->battery->SetPowerLoad( + this->dataPtr->consumerId, total_power_load); + if (!success) + ignerr << "Failed to set consumer power load." << std::endl; + // Start draining the battery if the robot has started moving if (!this->dataPtr->startDraining) { diff --git a/src/systems/battery_plugin/LinearBatteryPlugin.hh b/src/systems/battery_plugin/LinearBatteryPlugin.hh index a8886a073d..f82afa7515 100644 --- a/src/systems/battery_plugin/LinearBatteryPlugin.hh +++ b/src/systems/battery_plugin/LinearBatteryPlugin.hh @@ -58,11 +58,17 @@ namespace systems /// (Required if `` is set to true) /// - `` True to change the battery behavior to fix some issues /// described in https://github.com/gazebosim/gz-sim/issues/225. + /// - `` Whether to start draining the battery right away. + /// False by default. /// - `` A topic that is used to start battery - /// discharge. Any message on the specified topic will cause the batter to + /// discharge. Any message on the specified topic will cause the battery to /// start draining. This element can be specified multiple times if /// multiple topics should be monitored. Note that this mechanism will /// start the battery draining, and once started will keep drainig. + /// - `` A topic that is used to stop battery + /// discharge. Any message on the specified topic will cause the battery to + /// stop draining. + class LinearBatteryPlugin : public System, public ISystemConfigure, diff --git a/src/systems/environmental_sensor_system/CMakeLists.txt b/src/systems/environmental_sensor_system/CMakeLists.txt index 89f30b71dd..c589a464bf 100644 --- a/src/systems/environmental_sensor_system/CMakeLists.txt +++ b/src/systems/environmental_sensor_system/CMakeLists.txt @@ -6,3 +6,10 @@ gz_add_system(environmental-sensor gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-math${GZ_MATH_VER}::eigen3 ) + +gz_build_tests(TYPE UNIT + SOURCES + TransformTypes_TEST.cc + LIB_DEPS + gz-math${GZ_MATH_VER}::gz-math${GZ_MATH_VER} +) diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc index a69f9c0129..f1fd42ae56 100644 --- a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.cc @@ -15,6 +15,7 @@ * */ #include "EnvironmentalSensorSystem.hh" +#include "TransformTypes.hh" #include @@ -27,6 +28,7 @@ #include #include #include +#include #include #include @@ -76,19 +78,89 @@ class EnvironmentalSensor : public gz::sensors::Sensor // Load common sensor params gz::sensors::Sensor::Load(_sdf); - this->pub = this->node.Advertise(this->Topic()); + // Handle the field type + if (_sdf.Element() != nullptr && + _sdf.Element()->HasElement("output_format")) + { + std::string format = _sdf.Element()->Get("output_format"); + if (format == "scalar") + { + this->numberOfFields = 1; + } + else if (format == "vector3") + { + this->numberOfFields = 3; + } + else + { + gzerr << "Invalid output format " << format << "given." + << "valid options are scalar or vector3. " + << "Defaulting to 1." << std::endl; + } + } + + // Handle the transform type + if (_sdf.Element() != nullptr && + _sdf.Element()->HasElement("transform_type")) + { + auto res = + getTransformType(_sdf.Element()->Get("transform_type")); + + if (!res.has_value()) + { + gzerr << "Invalid transform type of " + << _sdf.Element()->Get("transform_type") + << " given. Valid types are ADD_VELOCITY_LOCAL, ADD_VELOCITY_GLOBAL," + << " LOCAL, and GLOBAL. Defaulting to GLOBAL." << std::endl; + } + else + { + this->transformType = res.value(); + } + } + + switch(this->numberOfFields) { + case 1: + this->pub = this->node.Advertise(this->Topic()); + break; + case 3: + this->pub = this->node.Advertise(this->Topic()); + break; + default: + gzerr << "Unable to create publisher too many fields" << "\n"; + } // If "environment_variable" is defined then remap // sensor from existing data. if (_sdf.Element() != nullptr && + this->numberOfFields == 1 && _sdf.Element()->HasElement("environment_variable")) { - this->fieldName = + this->fieldName[0] = _sdf.Element()->Get("environment_variable"); } + else if (_sdf.Element() != nullptr && + this->numberOfFields == 3) + { + if (_sdf.Element()->HasElement("environment_variable_x")) + { + this->fieldName[0] = + _sdf.Element()->Get("environment_variable_x"); + } + if (_sdf.Element()->HasElement("environment_variable_y")) + { + this->fieldName[1] = + _sdf.Element()->Get("environment_variable_y"); + } + if (_sdf.Element()->HasElement("environment_variable_z")) + { + this->fieldName[2] = + _sdf.Element()->Get("environment_variable_z"); + } + } else { - this->fieldName = type.substr(strlen(SENSOR_TYPE_PREFIX)); + this->fieldName[0] = type.substr(strlen(SENSOR_TYPE_PREFIX)); } // Allow setting custom frame_ids @@ -98,8 +170,18 @@ class EnvironmentalSensor : public gz::sensors::Sensor this->frameId = _sdf.Element()->Get("frame_id"); } - gzdbg << "Loaded environmental sensor for " << this->fieldName - << " publishing on " << this->Topic() << std::endl; + if (this->numberOfFields == 1) + { + gzdbg << "Loaded environmental sensor for " << this->fieldName[0] + << " publishing on " << this->Topic() << std::endl; + } + else + { + gzdbg << "Loaded environmental sensor for [" << this->fieldName[0] << ", " + << this->fieldName[1] << ", " << this->fieldName[2] + << "] publishing on " << this->Topic() << std::endl; + } + return true; } @@ -113,30 +195,72 @@ class EnvironmentalSensor : public gz::sensors::Sensor { if (!this->ready) return false; - if (!this->session.has_value()) return false; - // Step time if its not static - if (!this->gridField->staticTime) - this->session = this->gridField->frame[this->fieldName].StepTo( - this->session.value(), std::chrono::duration(_now).count()); + std::optional dataPoints[3]; + for (std::size_t i = 0; i < this->numberOfFields; ++i) + { + if (this->fieldName[i] == "") + { + // Empty field name means the column should default to zero. + dataPoints[i] = 0; + continue; + } + + if (!this->session[i].has_value()) return false; + + // Step time if its not static + if (!this->gridField->staticTime) + this->session[i] = this->gridField->frame[this->fieldName[i]].StepTo( + this->session[i].value(), + std::chrono::duration(_now).count()); + + if (!this->session[i].has_value()) return false; - if (!this->session.has_value()) return false; + dataPoints[i] = this->gridField->frame[this->fieldName[i]].LookUp( + this->session[i].value(), this->position); + } - gz::msgs::Double msg; - *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); - auto frame = msg.mutable_header()->add_data(); - frame->set_key("frame_id"); - frame->add_value((this->frameId == "") ? this->Name() : this->frameId); - auto data = this->gridField->frame[this->fieldName].LookUp( - this->session.value(), this->position); - if (!data.has_value()) + if (this->numberOfFields == 1) { + gz::msgs::Double msg; + *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value((this->frameId == "") ? this->Name() : this->frameId); + auto data = dataPoints[0]; + if (!data.has_value()) + { + gzwarn << "Failed to acquire value perhaps out of field?\n"; + return false; + } + msg.set_data(data.value()); + // TODO(anyone) Add sensor noise. + this->pub.Publish(msg); + } + else if (this->numberOfFields == 3) { - gzwarn << "Failed to acquire value perhaps out of field?\n"; - return false; + gz::msgs::Vector3d msg; + *msg.mutable_header()->mutable_stamp() = gz::msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value((this->frameId == "") ? this->Name() : this->frameId); + + if (!dataPoints[0].has_value() || !dataPoints[1].has_value() + || !dataPoints[2].has_value()) + { + gzwarn << "Failed to acquire value perhaps out of field?\n"; + return false; + } + + math::Vector3d vector( + dataPoints[0].value(), dataPoints[1].value(), dataPoints[2].value()); + auto transformed = transformFrame(this->transformType, this->objectPose, + this->velocity, vector); + + msg.set_x(transformed.X()); + msg.set_y(transformed.Y()); + msg.set_z(transformed.Z()); + this->pub.Publish(msg); } - msg.set_data(data.value()); - // TODO(anyone) Add sensor noise. - this->pub.Publish(msg); return true; } @@ -150,28 +274,42 @@ class EnvironmentalSensor : public gz::sensors::Sensor { gzdbg << "Setting new data table\n"; auto data = _data->Data(); - if(!data->frame.Has(this->fieldName)) + for (std::size_t i = 0; i < this->numberOfFields; ++i) { - gzwarn << "Environmental sensor could not find field " - << this->fieldName << "\n"; - this->ready = false; - return; + // If field name is empty it was intentionally left out so the column + // should default to zero. Otherwise if wrong name throw an error and + // disable output. + if (this->fieldName[i] != "" && !data->frame.Has(this->fieldName[i])) + { + gzwarn << "Environmental sensor could not find field " + << this->fieldName[i] << "\n"; + this->ready = false; + return; + } } this->gridField = data; - this->session = this->gridField->frame[this->fieldName].CreateSession(); - if (!this->gridField->staticTime) + for (std::size_t i = 0; i < this->numberOfFields; ++i) { - this->session = this->gridField->frame[this->fieldName].StepTo( - *this->session, - std::chrono::duration(_curr_time).count()); - } - this->ready = true; + if (this->fieldName[i] == "") + continue; - if(!this->session.has_value()) - { - gzerr << "Exceeded time stamp." << std::endl; + this->session[i] = + this->gridField->frame[this->fieldName[i]].CreateSession(); + if (!this->gridField->staticTime) + { + this->session[i] = this->gridField->frame[this->fieldName[i]].StepTo( + *this->session[i], + std::chrono::duration(_curr_time).count()); + } + + if(!this->session[i].has_value()) + { + gzerr << "Exceeded time stamp." << std::endl; + } } + + this->ready = true; } //////////////////////////////////////////////////////////////// @@ -186,7 +324,8 @@ class EnvironmentalSensor : public gz::sensors::Sensor { if (!this->ready) return false; - const auto worldPosition = worldPose(_entity, _ecm).Pos(); + this->objectPose = worldPose(_entity, _ecm); + const auto worldPosition = this->objectPose.Pos(); auto lookupCoords = getGridFieldCoordinates(_ecm, worldPosition, this->gridField); @@ -195,6 +334,11 @@ class EnvironmentalSensor : public gz::sensors::Sensor return false; } + auto vel = _ecm.Component(_entity); + if (vel != nullptr) + { + this->velocity = vel->Data(); + } this->position = lookupCoords.value(); return true; } @@ -202,16 +346,19 @@ class EnvironmentalSensor : public gz::sensors::Sensor //////////////////////////////////////////////////////////////// public: std::string Field() const { - return fieldName; + return fieldName[0]; } private: bool ready {false}; - private: math::Vector3d position; - private: std::string fieldName; + private: math::Vector3d position, velocity; + private: math::Pose3d objectPose; + private: std::size_t numberOfFields{1}; + private: std::string fieldName[3]; private: std::string frameId; - private: std::optional> session; + private: std::optional> session[3]; private: std::shared_ptr gridField; + private: TransformType transformType{TransformType::GLOBAL}; }; class gz::sim::EnvironmentalSensorSystemPrivate { @@ -313,6 +460,8 @@ void EnvironmentalSensorSystem::PreUpdate(const gz::sim::UpdateInfo &_info, gzerr << "No sensor data loaded\n"; } + enableComponent(_ecm, _entity); + // Keep track of this sensor this->dataPtr->entitySensorMap.insert(std::make_pair(_entity, std::move(sensor))); diff --git a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh index ecb7ad9845..3570f25037 100644 --- a/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh +++ b/src/systems/environmental_sensor_system/EnvironmentalSensorSystem.hh @@ -34,6 +34,36 @@ class EnvironmentalSensorSystemPrivate; /// field_name refers to the type of data you would like to output. /// Alternatively, if you would like to specify a custom field name you may do /// so using the tag. +/// +/// Additionally, the environment sensor supports scenarios where the data is in +/// the form of vector fields. For instance in the case of wind or ocean +/// currents. +/// +/// Tags: +/// - Either "scalar" or "vector3" depending on type +/// of output data desired. +/// - Only for scalar type. The name of the column of +/// the CSV file to be used. +/// - The name of the field to be used as the x value +/// in global frame for vector3 fields. Note: If this is left out and +/// vector3 is set, then the x value defaults to zero. +/// - The name of the field to be used as the y value +/// in global frame for vector3 fields. Note: If this is left out and +/// vector3 is set, then the y value defaults to zero. +/// - The name of the field to be used as the z value +/// in global frame vector3 fields. Note: If this is left out and +/// vector3 is set, then the z value defaults to zero. +/// - When handling vector2 or vector3 types it may be in our +/// interest to transform them to local coordinate frames. For instance +/// measurements of ocean currents or wind depend on the orientation and +/// velocity of the sensor. This field can have 4 values: +/// * GLOBAL - Don't transform the vectors to a local frame. +/// * LOCAL - Transform the vector to a local frame. +/// * ADD_VELOCITY_GLOBAL - Don't transform to local frame but account for +/// velocity change. +/// * ADD_VELOCITY_LOCAL - Transform to local frame and account for sensor +/// velocity. If you're working with wind or ocean currents, this is +/// probably the option you want. class EnvironmentalSensorSystem: public gz::sim::System, public gz::sim::ISystemConfigure, diff --git a/src/systems/environmental_sensor_system/TransformTypes.hh b/src/systems/environmental_sensor_system/TransformTypes.hh new file mode 100644 index 0000000000..94104f02bb --- /dev/null +++ b/src/systems/environmental_sensor_system/TransformTypes.hh @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_ENVIRONMENTAL_SYSTEM_TRANFORM_TYPE_HH_ +#define GZ_ENVIRONMENTAL_SYSTEM_TRANFORM_TYPE_HH_ + +#include +#include + +#include +#include + +namespace gz { +namespace sim { +/// \brief Transform Type +enum TransformType { + /// \brief Add the sensor's velocity and retrieve the value after + /// transforming the vector to the local frame. (Example would be for a + /// wind speed sensor or current sensor). + ADD_VELOCITY_LOCAL, + /// \brief Add the sensor's velocity and retrieve the value in a global + /// frame. (For instance to get Airspeed in a global frame) + ADD_VELOCITY_GLOBAL, + /// \brief The vector doesn't change with speed but we want it in local + /// frame. Example could be magnetic field lines in local frame. + LOCAL, + /// \brief The vector doesn't change with speed but we want it in local + /// frame. Example could be magnetic field lines in global frame. + GLOBAL +}; + +/// \brief Given a string return the type of transform +/// \param[in] _str - input string +/// \return std::nullopt if string invalid, else corresponding transform +std::optional getTransformType(const std::string &_str) +{ + if(_str == "ADD_VELOCITY_LOCAL") + return TransformType::ADD_VELOCITY_LOCAL; + if(_str == "ADD_VELOCITY_GLOBAL") + return TransformType::ADD_VELOCITY_GLOBAL; + if(_str == "LOCAL") + return TransformType::LOCAL; + if(_str == "GLOBAL") + return TransformType::GLOBAL; + return std::nullopt; +} + +/// \brief Given a string return the type of transform +/// \param[in] _type - Transform type. +/// \param[in] _pose - Global pose of frame to be transformed into. +/// \param[in] _velocity - Velocity of current frame. +/// \param[in] _reading - vector to be transformed. +/// \return transformed vector. +math::Vector3d transformFrame( + const TransformType _type, const math::Pose3d& _pose, + const math::Vector3d& _velocity, const math::Vector3d& _reading) +{ + math::Vector3d result; + math::Vector3d offset{0, 0, 0}; + + if (_type == ADD_VELOCITY_LOCAL || _type == ADD_VELOCITY_GLOBAL) + { + offset = -_velocity; + } + + switch (_type) { + case ADD_VELOCITY_LOCAL: + case LOCAL: + result = _pose.Rot().Inverse() * (_reading + offset); + break; + default: + result = (_reading + offset); + } + + return result; +} +} +} +#endif diff --git a/src/systems/environmental_sensor_system/TransformTypes_TEST.cc b/src/systems/environmental_sensor_system/TransformTypes_TEST.cc new file mode 100644 index 0000000000..452952d0b5 --- /dev/null +++ b/src/systems/environmental_sensor_system/TransformTypes_TEST.cc @@ -0,0 +1,73 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#include +#include "TransformTypes.hh" + +using namespace gz; +using namespace sim; + +TEST(TransformationTypes, transformFrame) +{ + // Create a vehicle rotated 90 degrees in the yaw axis + math::Pose3d pose(math::Vector3d(0, 0, 0), math::Quaterniond(0, 0, GZ_PI_2)); + + // Vehicle should be moving at 1m/s along x-axis (In global frame) + math::Vector3d velocity(1, 0, 0); + + // Create a current moving against the vehicle + math::Vector3d current(-3, 0, 0); + + // Test transforms + // Global transformFrame should keep things unaffected. + auto res = transformFrame(TransformType::GLOBAL, pose, velocity, current); + EXPECT_EQ(res, current); + + res = transformFrame( + TransformType::ADD_VELOCITY_GLOBAL, pose, velocity, current); + EXPECT_EQ(res, math::Vector3d(-4, 0, 0)); + + // Tranforming to local frame without accounting for velocity. Considering + // robot is facing Y axis. + // Top down view: + // vvvvvvvvv Current direction + // ^ (Global X, Robot -Y, Robot Direction of motion) + // | + // | Robot coords + // -----------> (Global Y, Robot +X heading) --->+x + // | | + // | v +y + // + res = transformFrame(TransformType::LOCAL, pose, velocity, current); + EXPECT_LT((res - math::Vector3d(0, 3, 0)).Length(), 1e-5); + + // Tranforming with vehicle's velocity in account + res = transformFrame( + TransformType::ADD_VELOCITY_LOCAL, pose, velocity, current); + EXPECT_LT((res - math::Vector3d(0, 4, 0)).Length(), 1e-5); +} + +TEST(TransformationTypes, getTransformType) +{ + EXPECT_EQ( + getTransformType("ADD_VELOCITY_LOCAL"), TransformType::ADD_VELOCITY_LOCAL); + EXPECT_EQ( + getTransformType("ADD_VELOCITY_GLOBAL"), + TransformType::ADD_VELOCITY_GLOBAL); + EXPECT_EQ(getTransformType("LOCAL"), TransformType::LOCAL); + EXPECT_EQ(getTransformType("GLOBAL"), TransformType::GLOBAL); + EXPECT_EQ(getTransformType("nonsense"), std::nullopt); +} diff --git a/src/systems/hydrodynamics/Hydrodynamics.cc b/src/systems/hydrodynamics/Hydrodynamics.cc index 933966feb8..6b2167a072 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.cc +++ b/src/systems/hydrodynamics/Hydrodynamics.cc @@ -24,6 +24,7 @@ #include #include "gz/sim/components/AngularVelocity.hh" +#include "gz/sim/components/Environment.hh" #include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/World.hh" @@ -78,14 +79,136 @@ class gz::sim::systems::HydrodynamicsPrivateData /// \brief Previous state. public: Eigen::VectorXd prevState; + /// \brief Previous derivative of the state public: Eigen::VectorXd prevStateDot; - /// Link entity + /// \brief Use current table if true + public: bool useCurrentTable {false}; + + /// \brief Current table xComponent + public: std::string axisComponents[3]; + + public: std::shared_ptr + gridField; + + public: std::optional> session[3]; + + /// \brief Link entity public: Entity linkEntity; /// \brief Ocean current callback public: void UpdateCurrent(const msgs::Vector3d &_msg); + ///////////////////////////////////////////////// + /// \brief Set the current table + /// \param[in] _ecm - The Entity Component Manager + /// \param[in] _currTime - The current time + public: void SetWaterCurrentTable( + const EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_currTime) + { + _ecm.EachNew([&](const Entity &/*_entity*/, + const components::Environment *_environment) -> bool + { + this->gridField = _environment->Data(); + + for (std::size_t i = 0; i < 3; i++) + { + if (!this->axisComponents[i].empty()) + { + if (!this->gridField->frame.Has(this->axisComponents[i])) + { + gzwarn << "Environmental sensor could not find field " + << this->axisComponents[i] << "\n"; + continue; + } + + this->session[i] = + this->gridField->frame[this->axisComponents[i]].CreateSession(); + if (!this->gridField->staticTime) + { + this->session[i] = + this->gridField->frame[this->axisComponents[i]].StepTo( + *this->session[i], + std::chrono::duration(_currTime).count()); + } + + if(!this->session[i].has_value()) + { + gzerr << "Exceeded time stamp." << std::endl; + } + } + } + return true; + }); + } + + ///////////////////////////////////////////////// + /// \brief Retrieve water current data from the environment + /// \param[in] _ecm - The Entity Component Manager + /// \param[in] _currTime - The current time + /// \param[in] _position - Position of the vehicle in world coordinates. + /// \return The current vector to be applied at this position and time. + public: math::Vector3d GetWaterCurrentFromEnvironment( + const EntityComponentManager &_ecm, + const std::chrono::steady_clock::duration &_currTime, + math::Vector3d _position) + { + math::Vector3d current(0, 0, 0); + + if (!this->gridField || + !(this->session[0].has_value() || + this->session[1].has_value() || this->session[2].has_value())) + { + return current; + } + + for (std::size_t i = 0; i < 3; i++) + { + if (!this->axisComponents[i].empty()) + { + if (!this->gridField->staticTime) + { + this->session[i] = + this->gridField->frame[this->axisComponents[i]].StepTo( + *this->session[i], + std::chrono::duration(_currTime).count()); + } + + if (!this->session[i].has_value()) + { + gzerr << "Time exceeded" << std::endl; + continue; + } + + auto position = getGridFieldCoordinates( + _ecm, _position, this->gridField); + + if (!position.has_value()) + { + gzerr << "Coordinate conversion failed" << std::endl; + continue; + } + + auto data = this->gridField->frame[this->axisComponents[i]].LookUp( + this->session[i].value(), position.value()); + if (!data.has_value()) + { + auto bounds = + this->gridField->frame[this->axisComponents[i]].Bounds( + this->session[i].value()); + gzwarn << "Failed to acquire value perhaps out of field?\n" + << "Bounds are " << bounds.first << ", " + << bounds.second << std::endl; + continue; + } + + current[i] = data.value(); + } + } + + return current; + } /// \brief Mutex public: std::mutex mtx; }; @@ -184,6 +307,8 @@ void Hydrodynamics::Configure( // Use SNAME 1950 convention to load the coeffecients. const auto snameConventionVel = "UVWPQR"; const auto snameConventionMoment = "xyzkmn"; + + bool warnBehaviourChange = false; for(auto i = 0; i < 6; i++) { for(auto j = 0; j < 6; j++) @@ -194,11 +319,17 @@ void Hydrodynamics::Configure( SdfParamDouble(_sdf, prefix, 0); for(auto k = 0; k < 6; k++) { + auto fieldName = prefix + snameConventionVel[k]; this->dataPtr->stabilityQuadraticDerivative[i*36 + j*6 + k] = SdfParamDouble( _sdf, - prefix + snameConventionVel[k], + fieldName, 0); + + if (_sdf->HasElement(fieldName)) { + warnBehaviourChange = true; + } + this->dataPtr->stabilityQuadraticAbsDerivative[i*36 + j*6 + k] = SdfParamDouble( _sdf, @@ -208,6 +339,16 @@ void Hydrodynamics::Configure( } } + + if (warnBehaviourChange) + { + ignwarn << "You are using parameters that may cause instabilities " + << "in your simulation. If your simulation crashes we recommend " + << "renaming -> and likewise for other axis " + << "for more information see:" << std::endl + << "\thttps://github.com/gazebosim/gz-sim/pull/1888" << std::endl; + } + // Added mass according to Fossen's equations (p 37) this->dataPtr->Ma = Eigen::MatrixXd::Zero(6, 6); for(auto i = 0; i < 6; i++) @@ -262,6 +403,28 @@ void Hydrodynamics::Configure( this->dataPtr->prevState = Eigen::VectorXd::Zero(6); + if(_sdf->HasElement("lookup_current_x")) + { + this->dataPtr->useCurrentTable = true; + this->dataPtr->axisComponents[0] = + _sdf->Get("lookup_current_x"); + } + + if(_sdf->HasElement("lookup_current_y")) + { + this->dataPtr->useCurrentTable = true; + this->dataPtr->axisComponents[1] = + _sdf->Get("lookup_current_y"); + } + + if(_sdf->HasElement("lookup_current_z")) + { + this->dataPtr->useCurrentTable = true; + this->dataPtr->axisComponents[2] = + _sdf->Get("lookup_current_z"); + } + + AddWorldPose(this->dataPtr->linkEntity, _ecm); AddAngularVelocityComponent(this->dataPtr->linkEntity, _ecm); AddWorldLinearVelocity(this->dataPtr->linkEntity, _ecm); @@ -272,6 +435,11 @@ void Hydrodynamics::PreUpdate( const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) { + if (this->dataPtr->useCurrentTable) + { + this->dataPtr->SetWaterCurrentTable(_ecm, _info.simTime); + } + if (_info.paused) return; @@ -300,7 +468,18 @@ void Hydrodynamics::PreUpdate( } // Get current vector - math::Vector3d currentVector; + math::Vector3d currentVector(0, 0, 0); + + if (this->dataPtr->useCurrentTable) + { + auto position = baseLink.WorldInertialPose(_ecm); + if (position.has_value()) + { + currentVector = this->dataPtr->GetWaterCurrentFromEnvironment( + _ecm, _info.simTime, position.value().Pos()); + } + } + else { std::lock_guard lock(this->dataPtr->mtx); currentVector = this->dataPtr->currentVector; @@ -401,10 +580,22 @@ void Hydrodynamics::PreUpdate( pose->Rot()*totalTorque); } +///////////////////////////////////////////////// +void Hydrodynamics::PostUpdate( + const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) +{ + if (this->dataPtr->useCurrentTable) + { + this->dataPtr->SetWaterCurrentTable(_ecm, _info.simTime); + } +} + GZ_ADD_PLUGIN( Hydrodynamics, System, Hydrodynamics::ISystemConfigure, - Hydrodynamics::ISystemPreUpdate + Hydrodynamics::ISystemPreUpdate, + Hydrodynamics::ISystemPostUpdate ) GZ_ADD_PLUGIN_ALIAS( diff --git a/src/systems/hydrodynamics/Hydrodynamics.hh b/src/systems/hydrodynamics/Hydrodynamics.hh index c8287b8b26..aab1a7cdb0 100644 --- a/src/systems/hydrodynamics/Hydrodynamics.hh +++ b/src/systems/hydrodynamics/Hydrodynamics.hh @@ -44,6 +44,7 @@ namespace systems /// The exact description of these parameters can be found on p. 37 and /// p. 43 of Fossen's book. They are used to calculate added mass, linear and /// quadratic drag and coriolis force. + /// /// ### Diagonal terms: /// * - Added mass in x direction [kg] /// * - Added mass in y direction [kg] @@ -51,18 +52,19 @@ namespace systems /// * - Added mass in roll direction [kgm^2] /// * - Added mass in pitch direction [kgm^2] /// * - Added mass in yaw direction [kgm^2] - /// * - Quadratic damping, 2nd order, x component [kg/m] + /// * - Quadratic damping, 2nd order, x component [kg/m] /// * - Linear damping, 1st order, x component [kg] - /// * - Quadratic damping, 2nd order, y component [kg/m] + /// * - Quadratic damping, 2nd order, y component [kg/m] /// * - Linear damping, 1st order, y component [kg] - /// * - Quadratic damping, 2nd order, z component [kg/m] + /// * - Quadratic damping, 2nd order, z component [kg/m] /// * - Linear damping, 1st order, z component [kg] - /// * - Quadratic damping, 2nd order, roll component [kg/m^2] + /// * - Quadratic damping, 2nd order, roll component [kg/m^2] /// * - Linear damping, 1st order, roll component [kg/m] - /// * - Quadratic damping, 2nd order, pitch component [kg/m^2] + /// * - Quadratic damping, 2nd order, pitch component [kg/m^2] /// * - Linear damping, 1st order, pitch component [kg/m] - /// * - Quadratic damping, 2nd order, yaw component [kg/m^2] + /// * - Quadratic damping, 2nd order, yaw component [kg/m^2] /// * - Linear damping, 1st order, yaw component [kg/m] + /// /// ### Cross terms /// In general we support cross terms as well. These are terms which act on /// non-diagonal sides. We use the SNAMe convention of naming search terms. @@ -72,7 +74,12 @@ namespace systems /// and yaw axis respectively. /// * Added Mass: <{x|y|z|k|m|n}Dot{U|V|W|P|Q|R}> e.g. /// Units are either kg or kgm^2 depending on the choice of terms. - /// * Quadratic Damping: <{x|y|z|k|m|n}{U|V|W|P|Q|R}{U|V|W|P|Q|R}> + /// * Quadratic Damping With abs term (this is probably what you want): + /// <{x|y|z|k|m|n}{U|V|W|P|Q|R}abs{U|V|W|P|Q|R}> + /// e.g. + /// Units are either kg/m or kg/m^2. + /// * Quadratic Damping (could lead to unwanted oscillations): + /// <{x|y|z|k|m|n}{U|V|W|P|Q|R}{U|V|W|P|Q|R}> /// e.g. /// Units are either kg/m or kg/m^2. /// * Linear Damping: <{x|y|z|k|m|n}{U|V|W|P|Q|R}>. e.g. @@ -94,6 +101,17 @@ namespace systems /// * - Disable Added Mass [Boolean, Default: false]. /// To be deprecated in Garden. /// + /// ### Loading external currents + /// One can use the EnvironmentPreload system to preload currents into the + /// plugin using data files. To use the data you may give CSV column names by + /// using `lookup_current_*` tags listed below: + /// * - X axis to use for lookup current + /// * - Y axis to use for lookup current + /// * - Z axis to use for lookup current + /// If any one of the fields is present, it is assumed current is to be loaded + /// by a data file and the topic will be ignored. If one or two fields are + /// present, the missing fields are assumed to default to zero. + /// /// # Example /// An example configuration is provided in the examples folder. The example /// uses the LiftDrag plugin to apply steering controls. It also uses the @@ -129,7 +147,8 @@ namespace systems class Hydrodynamics: public gz::sim::System, public gz::sim::ISystemConfigure, - public gz::sim::ISystemPreUpdate + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemPostUpdate { /// \brief Constructor public: Hydrodynamics(); @@ -149,6 +168,11 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) override; + /// Documentation inherited + public: void PostUpdate( + const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) override; + /// \brief Private data pointer private: std::unique_ptr dataPtr; }; diff --git a/src/systems/joint_controller/JointController.cc b/src/systems/joint_controller/JointController.cc index 5d519311ad..4c18ac7bdf 100644 --- a/src/systems/joint_controller/JointController.cc +++ b/src/systems/joint_controller/JointController.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -17,15 +18,18 @@ #include "JointController.hh" +#include #include #include +#include #include #include #include #include +#include "gz/sim/components/Actuators.hh" #include "gz/sim/components/JointForceCmd.hh" #include "gz/sim/components/JointVelocity.hh" #include "gz/sim/components/JointVelocityCmd.hh" @@ -41,14 +45,24 @@ class gz::sim::systems::JointControllerPrivate /// \param[in] _msg Velocity message public: void OnCmdVel(const msgs::Double &_msg); + /// \brief Callback for actuator velocity subscription + /// \param[in] _msg Velocity message + public: void OnActuatorVel(const msgs::Actuators &_msg); + /// \brief Gazebo communication node. public: transport::Node node; /// \brief Joint Entity - public: Entity jointEntity; + public: std::vector jointEntities; + + /// \brief Joint name + public: std::vector jointNames; /// \brief Commanded joint velocity - public: double jointVelCmd; + public: double jointVelCmd{0.0}; + + /// \brief Index of velocity actuator. + public: int actuatorNumber = 0; /// \brief mutex to protect jointVelCmd public: std::mutex jointVelCmdMutex; @@ -56,6 +70,9 @@ class gz::sim::systems::JointControllerPrivate /// \brief Model interface public: Model model{kNullEntity}; + /// \brief True if using Actuator msg to control joint velocity. + public: bool useActuatorMsg{false}; + /// \brief True if force commands are internally used to keep the target /// velocity. public: bool useForceCommands{false}; @@ -86,20 +103,22 @@ void JointController::Configure(const Entity &_entity, } // Get params from SDF - auto jointName = _sdf->Get("joint_name"); - if (jointName.empty()) + auto sdfElem = _sdf->FindElement("joint_name"); + while (sdfElem) { - gzerr << "JointController found an empty jointName parameter. " - << "Failed to initialize."; - return; + if (!sdfElem->Get().empty()) + { + this->dataPtr->jointNames.push_back(sdfElem->Get()); + } + else + { + gzerr << " provided but is empty." << std::endl; + } + sdfElem = sdfElem->GetNextElement("joint_name"); } - - this->dataPtr->jointEntity = this->dataPtr->model.JointByName(_ecm, - jointName); - if (this->dataPtr->jointEntity == kNullEntity) + if (this->dataPtr->jointNames.empty()) { - gzerr << "Joint with name[" << jointName << "] not found. " - << "The JointController may not control this joint.\n"; + gzerr << "Failed to get any ." << std::endl; return; } @@ -142,15 +161,65 @@ void JointController::Configure(const Entity &_entity, gzdbg << "[JointController] Velocity mode" << std::endl; } + if (_sdf->HasElement("use_actuator_msg") && + _sdf->Get("use_actuator_msg")) + { + if (_sdf->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + _sdf->Get("actuator_number"); + this->dataPtr->useActuatorMsg = true; + } + else + { + gzerr << "Please specify an actuator_number" << + "to use Actuator velocity message control." << std::endl; + } + } + // Subscribe to commands - std::string topic = transport::TopicUtils::AsValidTopic("/model/" + - this->dataPtr->model.Name(_ecm) + "/joint/" + jointName + - "/cmd_vel"); - if (topic.empty()) + std::string topic; + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (!this->dataPtr->useActuatorMsg)) { - gzerr << "Failed to create topic for joint [" << jointName - << "]" << std::endl; - return; + topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/joint/" + + this->dataPtr->jointNames[0] + "/cmd_vel"); + if (topic.empty()) + { + gzerr << "Failed to create topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (this->dataPtr->useActuatorMsg)) + { + topic = transport::TopicUtils::AsValidTopic("/actuators"); + if (topic.empty()) + { + gzerr << "Failed to create Actuator topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } + if (_sdf->HasElement("sub_topic")) + { + topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/" + + _sdf->Get("sub_topic")); + + if (topic.empty()) + { + gzerr << "Failed to create topic from sub_topic [/model/" + << this->dataPtr->model.Name(_ecm) << "/" + << _sdf->Get("sub_topic") + << "]" << " for joint [" << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } } if (_sdf->HasElement("topic")) { @@ -160,16 +229,30 @@ void JointController::Configure(const Entity &_entity, if (topic.empty()) { gzerr << "Failed to create topic [" << _sdf->Get("topic") - << "]" << " for joint [" << jointName + << "]" << " for joint [" << this->dataPtr->jointNames[0] << "]" << std::endl; return; } } - this->dataPtr->node.Subscribe(topic, &JointControllerPrivate::OnCmdVel, - this->dataPtr.get()); - gzmsg << "JointController subscribing to Double messages on [" << topic + if (this->dataPtr->useActuatorMsg) + { + this->dataPtr->node.Subscribe(topic, + &JointControllerPrivate::OnActuatorVel, + this->dataPtr.get()); + + gzmsg << "JointController subscribing to Actuator messages on [" << topic << "]" << std::endl; + } + else + { + this->dataPtr->node.Subscribe(topic, + &JointControllerPrivate::OnCmdVel, + this->dataPtr.get()); + + gzmsg << "JointController subscribing to Double messages on [" << topic + << "]" << std::endl; + } } ////////////////////////////////////////////////// @@ -178,10 +261,6 @@ void JointController::PreUpdate(const UpdateInfo &_info, { GZ_PROFILE("JointController::PreUpdate"); - // If the joint hasn't been identified yet, the plugin is disabled - if (this->dataPtr->jointEntity == kNullEntity) - return; - // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -190,19 +269,43 @@ void JointController::PreUpdate(const UpdateInfo &_info, << "s]. System may not work properly." << std::endl; } + // If the joints haven't been identified yet, look for them + if (this->dataPtr->jointEntities.empty()) + { + bool warned{false}; + for (const std::string &name : this->dataPtr->jointNames) + { + Entity joint = this->dataPtr->model.JointByName(_ecm, name); + if (joint != kNullEntity) + { + this->dataPtr->jointEntities.push_back(joint); + } + else if (!warned) + { + gzwarn << "Failed to find joint [" << name << "]" << std::endl; + warned = true; + } + } + } + if (this->dataPtr->jointEntities.empty()) + return; + // Nothing left to do if paused. if (_info.paused) return; // Create joint velocity component if one doesn't exist - auto jointVelComp = - _ecm.Component(this->dataPtr->jointEntity); - if (jointVelComp == nullptr) + auto jointVelComp = _ecm.Component( + this->dataPtr->jointEntities[0]); + if (!jointVelComp) { - _ecm.CreateComponent( - this->dataPtr->jointEntity, components::JointVelocity()); + _ecm.CreateComponent(this->dataPtr->jointEntities[0], + components::JointVelocity()); } - if (jointVelComp == nullptr) + + // We just created the joint velocity component, give one iteration for the + // physics system to update its size + if (jointVelComp == nullptr || jointVelComp->Data().empty()) return; double targetVel; @@ -211,43 +314,47 @@ void JointController::PreUpdate(const UpdateInfo &_info, targetVel = this->dataPtr->jointVelCmd; } + double error = jointVelComp->Data().at(0) - targetVel; + // Force mode. - if (this->dataPtr->useForceCommands) + if ((this->dataPtr->useForceCommands) && + (!jointVelComp->Data().empty())) { - if (!jointVelComp->Data().empty()) + for (Entity joint : this->dataPtr->jointEntities) { - double error = jointVelComp->Data().at(0) - targetVel; + // Update force command. double force = this->dataPtr->velPid.Update(error, _info.dt); auto forceComp = - _ecm.Component(this->dataPtr->jointEntity); + _ecm.Component(joint); if (forceComp == nullptr) { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointForceCmd({force})); + _ecm.CreateComponent(joint, + components::JointForceCmd({force})); } else { - forceComp->Data()[0] = force; + *forceComp = components::JointForceCmd({force}); } } } // Velocity mode. - else + else if (!this->dataPtr->useForceCommands) { // Update joint velocity - auto vel = - _ecm.Component(this->dataPtr->jointEntity); - - if (vel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->jointEntity, - components::JointVelocityCmd({targetVel})); - } - else if (!vel->Data().empty()) + for (Entity joint : this->dataPtr->jointEntities) { - vel->Data()[0] = targetVel; + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd({targetVel})); + } + else + { + *vel = components::JointVelocityCmd({targetVel}); + } } } } @@ -259,6 +366,20 @@ void JointControllerPrivate::OnCmdVel(const msgs::Double &_msg) this->jointVelCmd = _msg.data(); } +void JointControllerPrivate::OnActuatorVel(const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->jointVelCmdMutex); + if (this->actuatorNumber > _msg.velocity_size() - 1) + { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator velocity array which is of size " + << _msg.velocity_size() << std::endl; + return; + } + + this->jointVelCmd = static_cast(_msg.velocity(this->actuatorNumber)); +} + GZ_ADD_PLUGIN(JointController, System, JointController::ISystemConfigure, diff --git a/src/systems/joint_controller/JointController.hh b/src/systems/joint_controller/JointController.hh index 9bb10ece91..ae65473423 100644 --- a/src/systems/joint_controller/JointController.hh +++ b/src/systems/joint_controller/JointController.hh @@ -1,5 +1,6 @@ /* * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -37,14 +38,27 @@ namespace systems /// ## System Parameters /// /// `` The name of the joint to control. Required parameter. + /// Can also include multiple `` for identical joints. /// /// `` True to enable the controller implementation /// using force commands. If this parameter is not set or is false, the /// controller will use velocity commands internally. /// + /// `` True to enable the use of actuator message + /// for velocity command. The actuator msg is an array of floats for + /// position, velocity and normalized commands. Relies on + /// `` for the index of the velocity actuator and + /// defaults to topic "/actuators" when `topic` or `subtopic not set. + /// + /// `` used with `` to set + /// the index of the velocity actuator. + /// /// `` Topic to receive commands in. Defaults to /// `/model//joint//cmd_vel`. /// + /// `` Sub topic to receive commands in. + /// Defaults to "/model//". + /// /// `` Velocity to start with. /// /// ### Velocity mode: No additional parameters are required. diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 94b74e3735..d0280f6560 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -17,20 +18,25 @@ #include "JointPositionController.hh" +#include #include #include #include +#include #include #include #include #include +#include "gz/sim/components/Actuators.hh" +#include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointForceCmd.hh" #include "gz/sim/components/JointVelocityCmd.hh" #include "gz/sim/components/JointPosition.hh" #include "gz/sim/Model.hh" +#include "gz/sim/Util.hh" using namespace gz; using namespace sim; @@ -42,24 +48,34 @@ class gz::sim::systems::JointPositionControllerPrivate /// \param[in] _msg Position message public: void OnCmdPos(const msgs::Double &_msg); + /// \brief Callback for actuator position subscription + /// \param[in] _msg Position message + public: void OnActuatorPos(const msgs::Actuators &_msg); + /// \brief Gazebo communication node. public: transport::Node node; /// \brief Joint Entity - public: Entity jointEntity{kNullEntity}; + public: std::vector jointEntities; /// \brief Joint name - public: std::string jointName; + public: std::vector jointNames; /// \brief Commanded joint position public: double jointPosCmd{0.0}; + /// \brief Index of position actuator. + public: int actuatorNumber = 0; + /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; /// \brief Model interface public: Model model{kNullEntity}; + /// \brief True if using Actuator msg to control joint position. + public: bool useActuatorMsg{false}; + /// \brief Position PID controller. public: math::PID posPid; @@ -102,12 +118,22 @@ void JointPositionController::Configure(const Entity &_entity, } // Get params from SDF - this->dataPtr->jointName = _sdf->Get("joint_name"); - - if (this->dataPtr->jointName == "") + auto sdfElem = _sdf->FindElement("joint_name"); + while (sdfElem) { - gzerr << "JointPositionController found an empty jointName parameter. " - << "Failed to initialize."; + if (!sdfElem->Get().empty()) + { + this->dataPtr->jointNames.push_back(sdfElem->Get()); + } + else + { + gzerr << " provided but is empty." << std::endl; + } + sdfElem = sdfElem->GetNextElement("joint_name"); + } + if (this->dataPtr->jointNames.empty()) + { + gzerr << "Failed to get any ." << std::endl; return; } @@ -176,15 +202,66 @@ void JointPositionController::Configure(const Entity &_entity, this->dataPtr->jointPosCmd = _sdf->Get("initial_position"); } + if (_sdf->HasElement("use_actuator_msg") && + _sdf->Get("use_actuator_msg")) + { + if (_sdf->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + _sdf->Get("actuator_number"); + this->dataPtr->useActuatorMsg = true; + } + else + { + gzerr << "Please specify an actuator_number" << + "to use Actuator position message control." << std::endl; + } + } + // Subscribe to commands - std::string topic = transport::TopicUtils::AsValidTopic("/model/" + - this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName + - "/" + std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"); - if (topic.empty()) + std::string topic; + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (!this->dataPtr->useActuatorMsg)) { - gzerr << "Failed to create topic for joint [" << this->dataPtr->jointName - << "]" << std::endl; - return; + topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/joint/" + + this->dataPtr->jointNames[0] + "/" + + std::to_string(this->dataPtr->jointIndex) + "/cmd_pos"); + if (topic.empty()) + { + gzerr << "Failed to create topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } + if ((!_sdf->HasElement("sub_topic")) && (!_sdf->HasElement("topic")) + && (this->dataPtr->useActuatorMsg)) + { + topic = transport::TopicUtils::AsValidTopic("/actuators"); + if (topic.empty()) + { + gzerr << "Failed to create Actuator topic for joint [" + << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } + } + if (_sdf->HasElement("sub_topic")) + { + topic = transport::TopicUtils::AsValidTopic("/model/" + + this->dataPtr->model.Name(_ecm) + "/" + + _sdf->Get("sub_topic")); + + if (topic.empty()) + { + gzerr << "Failed to create topic from sub_topic [/model/" + << this->dataPtr->model.Name(_ecm) << "/" + << _sdf->Get("sub_topic") + << "]" << " for joint [" << this->dataPtr->jointNames[0] + << "]" << std::endl; + return; + } } if (_sdf->HasElement("topic")) { @@ -194,13 +271,29 @@ void JointPositionController::Configure(const Entity &_entity, if (topic.empty()) { gzerr << "Failed to create topic [" << _sdf->Get("topic") - << "]" << " for joint [" << this->dataPtr->jointName + << "]" << " for joint [" << this->dataPtr->jointNames[0] << "]" << std::endl; return; } } - this->dataPtr->node.Subscribe( - topic, &JointPositionControllerPrivate::OnCmdPos, this->dataPtr.get()); + if (this->dataPtr->useActuatorMsg) + { + this->dataPtr->node.Subscribe(topic, + &JointPositionControllerPrivate::OnActuatorPos, + this->dataPtr.get()); + + gzmsg << "JointPositionController subscribing to Actuator messages on [" + << topic << "]" << std::endl; + } + else + { + this->dataPtr->node.Subscribe(topic, + &JointPositionControllerPrivate::OnCmdPos, + this->dataPtr.get()); + + gzmsg << "JointPositionController subscribing to Double messages on [" + << topic << "]" << std::endl; + } gzdbg << "[JointPositionController] system parameters:" << std::endl; gzdbg << "p_gain: [" << p << "]" << std::endl; @@ -231,37 +324,68 @@ void JointPositionController::PreUpdate( << "s]. System may not work properly." << std::endl; } - // If the joint hasn't been identified yet, look for it - if (this->dataPtr->jointEntity == kNullEntity) + // If the joints haven't been identified yet, look for them + if (this->dataPtr->jointEntities.empty()) { - this->dataPtr->jointEntity = - this->dataPtr->model.JointByName(_ecm, this->dataPtr->jointName); + bool warned{false}; + for (const std::string &name : this->dataPtr->jointNames) + { + // First try to resolve by scoped name. + Entity joint = kNullEntity; + auto entities = entitiesFromScopedName( + name, _ecm, this->dataPtr->model.Entity()); + + if (!entities.empty()) + { + if (entities.size() > 1) + { + gzwarn << "Multiple joint entities with name [" + << name << "] found. " + << "Using the first one.\n"; + } + joint = *entities.begin(); + + // Validate + if (!_ecm.EntityHasComponentType(joint, components::Joint::typeId)) + { + gzerr << "Entity with name[" << name + << "] is not a joint\n"; + joint = kNullEntity; + } + else + { + gzdbg << "Identified joint [" << name + << "] as Entity [" << joint << "]\n"; + } + } + + if (joint != kNullEntity) + { + this->dataPtr->jointEntities.push_back(joint); + } + else if (!warned) + { + gzwarn << "Failed to find joint [" << name << "]\n"; + warned = true; + } + } } - - // If the joint is still not found then warn the user, they may have entered - // the wrong joint name. - if (this->dataPtr->jointEntity == kNullEntity) - { - static bool warned = false; - if(!warned) - gzerr << "Could not find joint with name [" - << this->dataPtr->jointName <<"]\n"; - warned = true; + if (this->dataPtr->jointEntities.empty()) return; - } // Nothing left to do if paused. if (_info.paused) return; // Create joint position component if one doesn't exist - auto jointPosComp = - _ecm.Component(this->dataPtr->jointEntity); - if (jointPosComp == nullptr) + auto jointPosComp = _ecm.Component( + this->dataPtr->jointEntities[0]); + if (!jointPosComp) { - _ecm.CreateComponent( - this->dataPtr->jointEntity, components::JointPosition()); + _ecm.CreateComponent(this->dataPtr->jointEntities[0], + components::JointPosition()); } + // We just created the joint position component, give one iteration for the // physics system to update its size if (jointPosComp == nullptr || jointPosComp->Data().empty()) @@ -271,15 +395,15 @@ void JointPositionController::PreUpdate( if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) { static std::unordered_set reported; - if (reported.find(this->dataPtr->jointEntity) == reported.end()) + if (reported.find(this->dataPtr->jointEntities[0]) == reported.end()) { gzerr << "[JointPositionController]: Detected an invalid " << "parameter. The index specified is [" << this->dataPtr->jointIndex << "] but joint [" - << this->dataPtr->jointName << "] only has [" + << this->dataPtr->jointNames[0] << "] only has [" << jointPosComp->Data().size() << "] index[es]. " << "This controller will be ignored" << std::endl; - reported.insert(this->dataPtr->jointEntity); + reported.insert(this->dataPtr->jointEntities[0]); } return; } @@ -315,37 +439,40 @@ void JointPositionController::PreUpdate( { targetVel = -error; } - - // Set velocity and return - auto vel = - _ecm.Component(this->dataPtr->jointEntity); - - if (vel == nullptr) - { - _ecm.CreateComponent( - this->dataPtr->jointEntity, - components::JointVelocityCmd({targetVel})); - } - else if (!vel->Data().empty()) + for (Entity joint : this->dataPtr->jointEntities) { - vel->Data()[0] = targetVel; + // Update velocity command. + auto vel = _ecm.Component(joint); + + if (vel == nullptr) + { + _ecm.CreateComponent( + joint, components::JointVelocityCmd({targetVel})); + } + else + { + *vel = components::JointVelocityCmd({targetVel}); + } } return; } - // Update force command. - double force = this->dataPtr->posPid.Update(error, _info.dt); - - auto forceComp = - _ecm.Component(this->dataPtr->jointEntity); - if (forceComp == nullptr) + for (Entity joint : this->dataPtr->jointEntities) { - _ecm.CreateComponent(this->dataPtr->jointEntity, - components::JointForceCmd({force})); - } - else - { - forceComp->Data()[this->dataPtr->jointIndex] = force; + // Update force command. + double force = this->dataPtr->posPid.Update(error, _info.dt); + + auto forceComp = + _ecm.Component(joint); + if (forceComp == nullptr) + { + _ecm.CreateComponent(joint, + components::JointForceCmd({force})); + } + else + { + *forceComp = components::JointForceCmd({force}); + } } } @@ -356,6 +483,20 @@ void JointPositionControllerPrivate::OnCmdPos(const msgs::Double &_msg) this->jointPosCmd = _msg.data(); } +void JointPositionControllerPrivate::OnActuatorPos(const msgs::Actuators &_msg) +{ + std::lock_guard lock(this->jointCmdMutex); + if (this->actuatorNumber > _msg.position_size() - 1) + { + gzerr << "You tried to access index " << this->actuatorNumber + << " of the Actuator position array which is of size " + << _msg.position_size() << std::endl; + return; + } + + this->jointPosCmd = static_cast(_msg.position(this->actuatorNumber)); +} + GZ_ADD_PLUGIN(JointPositionController, System, JointPositionController::ISystemConfigure, diff --git a/src/systems/joint_position_controller/JointPositionController.hh b/src/systems/joint_position_controller/JointPositionController.hh index 6f80d15c14..af139641d7 100644 --- a/src/systems/joint_position_controller/JointPositionController.hh +++ b/src/systems/joint_position_controller/JointPositionController.hh @@ -1,5 +1,6 @@ /* * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -35,7 +36,7 @@ namespace systems /// reference to a single joint. /// /// A new Gazebo Transport topic is created to send target joint positions. - /// The topic name is + /// The default topic name is /// "/model//joint///cmd_pos". /// /// This topic accepts gz::msgs::Double values representing the target @@ -46,10 +47,18 @@ namespace systems /// ## System Parameters /// /// `` The name of the joint to control. Required parameter. + /// Can also include multiple `` for identical joints. /// /// `` Axis of the joint to control. Optional parameter. /// The default value is 0. /// + /// `` True to enable the use of actutor message + /// for position command. Relies on `` for the + /// index of the position actuator and defaults to topic "/actuators". + /// + /// `` used with `` to set + /// the index of the position actuator. + /// /// `` The proportional gain of the PID. Optional parameter. /// The default value is 1. /// @@ -82,6 +91,9 @@ namespace systems /// here, otherwise the controller defaults to listening on /// "/model//joint///cmd_pos". /// + /// `` If you wish to listen on a sub_topic you may specify it + /// here "/model//". + /// /// `` Initial position of a joint. Optional parameter. /// The default value is 0. class JointPositionController diff --git a/src/systems/magnetometer/Magnetometer.cc b/src/systems/magnetometer/Magnetometer.cc index 8b28306ef4..499664a046 100644 --- a/src/systems/magnetometer/Magnetometer.cc +++ b/src/systems/magnetometer/Magnetometer.cc @@ -47,6 +47,75 @@ using namespace gz; using namespace sim; using namespace systems; +/** set this always to the sampling in degrees for the table below */ +static constexpr const float SAMPLING_RES = 10.0f; +static constexpr const float SAMPLING_MIN_LAT = -60.0f; +static constexpr const float SAMPLING_MAX_LAT = 60.0f; +static constexpr const float SAMPLING_MIN_LON = -180.0f; +static constexpr const float SAMPLING_MAX_LON = 180.0f; + +/// Returns scalar value constrained by (min_val, max_val) +template +static inline constexpr const Scalar &constrain( + const Scalar &val, const Scalar &min_val, const Scalar &max_val) +{ + return (val < min_val) ? min_val : ((val > max_val) ? max_val : val); +} + +// declination data in degrees +static constexpr const int8_t declination_table[13][37] = \ +{ + { 47, 46, 45, 43, 42, 41, 39, 37, 33, 29, 23, 16, 10, 4, -1, -6, -10, -15, -20, -27, -34, -42, -49, -56, -62, -67, -72, -74, -75, -73, -61, -22, 26, 42, 47, 48, 47 }, // NOLINT + { 31, 31, 31, 30, 30, 30, 30, 29, 27, 24, 18, 11, 3, -4, -9, -13, -15, -18, -21, -27, -33, -40, -47, -52, -56, -57, -56, -52, -44, -30, -14, 2, 14, 22, 27, 30, 31 }, // NOLINT + { 22, 23, 23, 23, 22, 22, 22, 23, 22, 19, 13, 5, -4, -12, -17, -20, -22, -22, -23, -25, -30, -36, -41, -45, -46, -44, -39, -31, -21, -11, -3, 4, 10, 15, 19, 21, 22 }, // NOLINT + { 17, 17, 17, 18, 17, 17, 17, 17, 16, 13, 8, -1, -10, -18, -22, -25, -26, -25, -22, -20, -21, -25, -29, -32, -31, -28, -23, -16, -9, -3, 0, 4, 7, 11, 14, 16, 17 }, // NOLINT + { 13, 13, 14, 14, 14, 13, 13, 12, 11, 9, 3, -5, -14, -20, -24, -25, -24, -21, -17, -12, -9, -11, -14, -17, -18, -16, -12, -8, -3, -0, 1, 3, 6, 8, 11, 12, 13 }, // NOLINT + { 11, 11, 11, 11, 11, 10, 10, 10, 9, 6, -0, -8, -15, -21, -23, -22, -19, -15, -10, -5, -2, -2, -4, -7, -9, -8, -7, -4, -1, 1, 1, 2, 4, 7, 9, 10, 11 }, // NOLINT + { 10, 9, 9, 9, 9, 9, 9, 8, 7, 3, -3, -10, -16, -20, -20, -18, -14, -9, -5, -2, 1, 2, 0, -2, -4, -4, -3, -2, -0, 0, 0, 1, 3, 5, 7, 9, 10 }, // NOLINT + { 9, 9, 9, 9, 9, 9, 9, 8, 6, 1, -4, -11, -16, -18, -17, -14, -10, -5, -2, -0, 2, 3, 2, 0, -1, -2, -2, -1, -0, -1, -1, -1, 1, 3, 6, 8, 9 }, // NOLINT + { 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -6, -12, -15, -16, -15, -11, -7, -4, -1, 1, 3, 4, 3, 2, 1, 0, -0, -0, -1, -2, -3, -4, -2, 0, 3, 6, 8 }, // NOLINT + { 7, 9, 10, 11, 12, 12, 12, 9, 5, -1, -7, -13, -15, -15, -13, -10, -6, -3, 0, 2, 3, 4, 4, 4, 3, 2, 1, 0, -1, -3, -5, -6, -6, -3, 0, 4, 7 }, // NOLINT + { 5, 8, 11, 13, 14, 15, 14, 11, 5, -2, -9, -15, -17, -16, -13, -10, -6, -3, 0, 3, 4, 5, 6, 6, 6, 5, 4, 2, -1, -5, -8, -9, -9, -6, -3, 1, 5 }, // NOLINT + { 3, 8, 11, 15, 17, 17, 16, 12, 5, -4, -12, -18, -19, -18, -16, -12, -8, -4, -0, 3, 5, 7, 9, 10, 10, 9, 7, 4, -1, -6, -10, -12, -12, -9, -5, -1, 3 }, // NOLINT + { 3, 8, 12, 16, 19, 20, 18, 13, 4, -8, -18, -24, -25, -23, -20, -16, -11, -6, -1, 3, 7, 11, 14, 16, 17, 17, 14, 8, -0, -8, -13, -15, -14, -11, -7, -2, 3 }, // NOLINT +}; + +// inclination data in degrees +static constexpr const int8_t inclination_table[13][37] = \ +{ + { -78, -76, -74, -72, -70, -68, -65, -63, -60, -57, -55, -54, -54, -55, -56, -57, -58, -59, -59, -59, -59, -60, -61, -63, -66, -69, -73, -76, -79, -83, -86, -87, -86, -84, -82, -80, -78 }, // NOLINT + { -72, -70, -68, -66, -64, -62, -60, -57, -54, -51, -49, -48, -49, -51, -55, -58, -60, -61, -61, -61, -60, -60, -61, -63, -66, -69, -72, -76, -78, -80, -81, -80, -79, -77, -76, -74, -72 }, // NOLINT + { -64, -62, -60, -59, -57, -55, -53, -50, -47, -44, -41, -41, -43, -47, -53, -58, -62, -65, -66, -65, -63, -62, -61, -63, -65, -68, -71, -73, -74, -74, -73, -72, -71, -70, -68, -66, -64 }, // NOLINT + { -55, -53, -51, -49, -46, -44, -42, -40, -37, -33, -30, -30, -34, -41, -48, -55, -60, -65, -67, -68, -66, -63, -61, -61, -62, -64, -65, -66, -66, -65, -64, -63, -62, -61, -59, -57, -55 }, // NOLINT + { -42, -40, -37, -35, -33, -30, -28, -25, -22, -18, -15, -16, -22, -31, -40, -48, -55, -59, -62, -63, -61, -58, -55, -53, -53, -54, -55, -55, -54, -53, -51, -51, -50, -49, -47, -45, -42 }, // NOLINT + { -25, -22, -20, -17, -15, -12, -10, -7, -3, 1, 3, 2, -5, -16, -27, -37, -44, -48, -50, -50, -48, -44, -41, -38, -38, -38, -39, -39, -38, -37, -36, -35, -35, -34, -31, -28, -25 }, // NOLINT + { -5, -2, 1, 3, 5, 8, 10, 13, 16, 20, 21, 19, 12, 2, -10, -20, -27, -30, -30, -29, -27, -23, -19, -17, -17, -17, -18, -18, -17, -16, -16, -16, -16, -15, -12, -9, -5 }, // NOLINT + { 15, 18, 21, 22, 24, 26, 29, 31, 34, 36, 37, 34, 28, 20, 10, 2, -3, -5, -5, -4, -2, 2, 5, 7, 8, 7, 7, 6, 7, 7, 7, 6, 5, 6, 8, 11, 15 }, // NOLINT + { 31, 34, 36, 38, 39, 41, 43, 46, 48, 49, 49, 46, 42, 36, 29, 24, 20, 19, 20, 21, 23, 25, 28, 30, 30, 30, 29, 29, 29, 29, 28, 27, 25, 25, 26, 28, 31 }, // NOLINT + { 43, 45, 47, 49, 51, 53, 55, 57, 58, 59, 59, 56, 53, 49, 45, 42, 40, 40, 40, 41, 43, 44, 46, 47, 47, 47, 47, 47, 47, 47, 46, 44, 42, 41, 40, 42, 43 }, // NOLINT + { 53, 54, 56, 57, 59, 61, 64, 66, 67, 68, 67, 65, 62, 60, 57, 55, 55, 54, 55, 56, 57, 58, 59, 59, 60, 60, 60, 60, 60, 60, 59, 57, 55, 53, 52, 52, 53 }, // NOLINT + { 62, 63, 64, 65, 67, 69, 71, 73, 75, 75, 74, 73, 70, 68, 67, 66, 65, 65, 65, 66, 66, 67, 68, 68, 69, 70, 70, 71, 71, 70, 69, 67, 65, 63, 62, 62, 62 }, // NOLINT + { 71, 71, 72, 73, 75, 77, 78, 80, 81, 81, 80, 79, 77, 76, 74, 73, 73, 73, 73, 73, 73, 74, 74, 75, 76, 77, 78, 78, 78, 78, 77, 75, 73, 72, 71, 71, 71 }, // NOLINT +}; + +// strength data in centi-Tesla +static constexpr const int8_t strength_table[13][37] = \ +{ + { 62, 60, 58, 56, 54, 52, 49, 46, 43, 41, 38, 36, 34, 32, 31, 31, 30, 30, 30, 31, 33, 35, 38, 42, 46, 51, 55, 59, 62, 64, 66, 67, 67, 66, 65, 64, 62 }, // NOLINT + { 59, 56, 54, 52, 50, 47, 44, 41, 38, 35, 32, 29, 28, 27, 26, 26, 26, 25, 25, 26, 28, 30, 34, 39, 44, 49, 54, 58, 61, 64, 65, 66, 65, 64, 63, 61, 59 }, // NOLINT + { 54, 52, 49, 47, 45, 42, 40, 37, 34, 30, 27, 25, 24, 24, 24, 24, 24, 24, 24, 24, 25, 28, 32, 37, 42, 48, 52, 56, 59, 61, 62, 62, 62, 60, 59, 56, 54 }, // NOLINT + { 49, 47, 44, 42, 40, 37, 35, 33, 30, 28, 25, 23, 22, 23, 23, 24, 25, 25, 26, 26, 26, 28, 31, 36, 41, 46, 51, 54, 56, 57, 57, 57, 56, 55, 53, 51, 49 }, // NOLINT + { 43, 41, 39, 37, 35, 33, 32, 30, 28, 26, 25, 23, 23, 23, 24, 25, 26, 28, 29, 29, 29, 30, 32, 36, 40, 44, 48, 51, 52, 52, 51, 51, 50, 49, 47, 45, 43 }, // NOLINT + { 38, 36, 35, 33, 32, 31, 30, 29, 28, 27, 26, 25, 24, 24, 25, 26, 28, 30, 31, 32, 32, 32, 33, 35, 38, 42, 44, 46, 47, 46, 45, 45, 44, 43, 41, 40, 38 }, // NOLINT + { 34, 33, 32, 32, 31, 31, 31, 30, 30, 30, 29, 28, 27, 27, 27, 28, 29, 31, 32, 33, 33, 33, 34, 35, 37, 39, 41, 42, 43, 42, 41, 40, 39, 38, 36, 35, 34 }, // NOLINT + { 33, 33, 32, 32, 33, 33, 34, 34, 35, 35, 34, 33, 32, 31, 30, 30, 31, 32, 33, 34, 35, 35, 36, 37, 38, 40, 41, 42, 42, 41, 40, 39, 37, 36, 34, 33, 33 }, // NOLINT + { 34, 34, 34, 35, 36, 37, 39, 40, 41, 41, 40, 39, 37, 35, 35, 34, 35, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 45, 45, 43, 41, 39, 37, 35, 34, 34 }, // NOLINT + { 37, 37, 38, 39, 41, 42, 44, 46, 47, 47, 46, 45, 43, 41, 40, 39, 39, 40, 41, 41, 42, 43, 45, 46, 47, 48, 49, 50, 50, 50, 48, 46, 43, 41, 39, 38, 37 }, // NOLINT + { 42, 42, 43, 44, 46, 48, 50, 52, 53, 53, 52, 51, 49, 47, 45, 45, 44, 44, 45, 46, 46, 47, 48, 50, 51, 53, 54, 55, 56, 55, 54, 52, 49, 46, 44, 43, 42 }, // NOLINT + { 48, 48, 49, 50, 52, 53, 55, 56, 57, 57, 56, 55, 53, 51, 50, 49, 48, 48, 48, 49, 49, 50, 51, 53, 55, 56, 58, 59, 60, 60, 58, 56, 54, 52, 50, 49, 48 }, // NOLINT + { 54, 54, 54, 55, 56, 57, 58, 58, 59, 58, 58, 57, 56, 54, 53, 52, 51, 51, 51, 51, 52, 53, 54, 55, 57, 58, 60, 61, 62, 61, 61, 59, 58, 56, 55, 54, 54 }, // NOLINT +}; + /// \brief Private Magnetometer data class. class gz::sim::systems::MagnetometerPrivate { @@ -90,6 +159,78 @@ class gz::sim::systems::MagnetometerPrivate /// from simulation. /// \param[in] _ecm Immutable reference to ECM. public: void RemoveMagnetometerEntities(const EntityComponentManager &_ecm); + + unsigned get_lookup_table_index(float &val, float min, float max) + { + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + // limit to table bounds - required for maxima even when table spans full + // globe range + // limit to (table bounds - 1) because bilinear interpolation requires + // checking (index + 1) + val = constrain(val, min, max - SAMPLING_RES); + + return static_cast((-(min) + val) / SAMPLING_RES); + } + + float get_table_data(float lat, float lon, const int8_t table[13][37]) + { + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + float min_lat = static_cast(lat / SAMPLING_RES) * SAMPLING_RES; + float min_lon = static_cast(lon / SAMPLING_RES) * SAMPLING_RES; + + /* find index of nearest low sampling point */ + unsigned min_lat_index = + get_lookup_table_index(min_lat, SAMPLING_MIN_LAT, SAMPLING_MAX_LAT); + unsigned min_lon_index = + get_lookup_table_index(min_lon, SAMPLING_MIN_LON, SAMPLING_MAX_LON); + + const float data_sw = table[min_lat_index][min_lon_index]; + const float data_se = table[min_lat_index][min_lon_index + 1]; + const float data_ne = table[min_lat_index + 1][min_lon_index + 1]; + const float data_nw = table[min_lat_index + 1][min_lon_index]; + + /* perform bilinear interpolation on the four grid corners */ + const float lat_scale = constrain( + (lat - min_lat) / SAMPLING_RES, 0.0f, 1.0f); + const float lon_scale = constrain( + (lon - min_lon) / SAMPLING_RES, 0.0f, 1.0f); + + const float data_min = lon_scale * (data_se - data_sw) + data_sw; + const float data_max = lon_scale * (data_ne - data_nw) + data_nw; + + return lat_scale * (data_max - data_min) + data_min; + } + + // return magnetic declination in degrees + float get_mag_declination(float lat, float lon) + { + return get_table_data(lat, lon, declination_table); + } + + // return magnetic field inclination in degrees + float get_mag_inclination(float lat, float lon) + { + return get_table_data(lat, lon, inclination_table); + } + + // return magnetic field strength in centi-Tesla + float get_mag_strength(float lat, float lon) + { + return get_table_data(lat, lon, strength_table); + } }; ////////////////////////////////////////////////// @@ -206,8 +347,6 @@ void MagnetometerPrivate::AddMagnetometer( _parent->Data())->Data(); sensor->SetParent(parentName); - // set world magnetic field. Assume uniform in world and does not - // change throughout simulation sensor->SetWorldMagneticField(_worldField->Data()); // Get initial pose of sensor and set the reference z pos @@ -286,6 +425,40 @@ void MagnetometerPrivate::Update( // Get the magnetometer physical position const math::Pose3d &magnetometerWorldPose = _worldPose->Data(); it->second->SetWorldPose(magnetometerWorldPose); + + // Position + auto latLonEle = sphericalCoordinates(_entity, _ecm); + if (!latLonEle) + { + gzwarn << "Failed to update NavSat sensor enity [" << _entity + << "]. Spherical coordinates not set." << std::endl; + return true; + } + + auto lat_rad = GZ_DTOR(latLonEle.value().X()); + auto lon_rad = GZ_DTOR(latLonEle.value().Y()); + + // Magnetic declination and inclination (radians) + float declination_rad = + get_mag_declination( + lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI) * GZ_PI / 180; + float inclination_rad = + get_mag_inclination( + lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI) * GZ_PI / 180; + + // Magnetic strength (10^5xnanoTesla) + float strength_ga = + 0.01f * + get_mag_strength(lat_rad * 180 / GZ_PI, lon_rad * 180 / GZ_PI); + + // Magnetic filed components are calculated by http://geomag.nrcan.gc.ca/mag_fld/comp-en.php + float H = strength_ga * cosf(inclination_rad); + float Z = tanf(inclination_rad) * H; + float X = H * cosf(declination_rad); + float Y = H * sinf(declination_rad); + + math::Vector3d magnetic_field_I(X, Y, Z); + it->second->SetWorldMagneticField(magnetic_field_I); } else { diff --git a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc index b8609ede37..8e6e421ce9 100644 --- a/src/systems/multicopter_motor_model/MulticopterMotorModel.cc +++ b/src/systems/multicopter_motor_model/MulticopterMotorModel.cc @@ -157,8 +157,8 @@ class gz::sim::systems::MulticopterMotorModelPrivate /// \brief Sampling time (from motor_model.hpp). public: double samplingTime = 0.01; - /// \brief Index of motor on multirotor_base. - public: int motorNumber = 0; + /// \brief Index of motor in Actuators msg on multirotor_base. + public: int actuatorNumber = 0; /// \brief Turning direction of the motor. public: int turningDirection = turning_direction::kCw; @@ -283,11 +283,22 @@ void MulticopterMotorModel::Configure(const Entity &_entity, return; } - if (sdfClone->HasElement("motorNumber")) - this->dataPtr->motorNumber = + if (sdfClone->HasElement("actuator_number")) + { + this->dataPtr->actuatorNumber = + sdfClone->GetElement("actuator_number")->Get(); + } + else if (sdfClone->HasElement("motorNumber")) + { + this->dataPtr->actuatorNumber = sdfClone->GetElement("motorNumber")->Get(); + gzwarn << " is depricated, " + << "please use .\n"; + } else - gzerr << "Please specify a motorNumber.\n"; + { + gzerr << "Please specify a actuator_number.\n"; + } if (sdfClone->HasElement("turningDirection")) { @@ -512,9 +523,9 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( if (msg.has_value()) { - if (this->motorNumber > msg->velocity_size() - 1) + if (this->actuatorNumber > msg->velocity_size() - 1) { - gzerr << "You tried to access index " << this->motorNumber + gzerr << "You tried to access index " << this->actuatorNumber << " of the Actuator velocity array which is of size " << msg->velocity_size() << std::endl; return; @@ -523,13 +534,13 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( if (this->motorType == MotorType::kVelocity) { this->refMotorInput = std::min( - static_cast(msg->velocity(this->motorNumber)), + static_cast(msg->velocity(this->actuatorNumber)), static_cast(this->maxRotVelocity)); } // else if (this->motorType == MotorType::kPosition) else // if (this->motorType == MotorType::kForce) { { - this->refMotorInput = msg->velocity(this->motorNumber); + this->refMotorInput = msg->velocity(this->actuatorNumber); } } @@ -554,7 +565,7 @@ void MulticopterMotorModelPrivate::UpdateForcesAndMoments( double motorRotVel = jointVelocity->Data()[0]; if (motorRotVel / (2 * GZ_PI) > 1 / (2 * this->samplingTime)) { - gzerr << "Aliasing on motor [" << this->motorNumber + gzerr << "Aliasing on motor [" << this->actuatorNumber << "] might occur. Consider making smaller simulation time " "steps or raising the rotorVelocitySlowdownSim param.\n"; } diff --git a/src/systems/odometry_publisher/OdometryPublisher.cc b/src/systems/odometry_publisher/OdometryPublisher.cc index 8cb1aa8467..13bfb64514 100644 --- a/src/systems/odometry_publisher/OdometryPublisher.cc +++ b/src/systems/odometry_publisher/OdometryPublisher.cc @@ -467,6 +467,10 @@ void OdometryPublisherPrivate::UpdateOdometry( msgCovariance.mutable_pose_with_covariance()-> mutable_pose()->mutable_position()->set_z(msg.pose().position().z()); + // Copy orientation from odometry msg. + msgs::Set(msgCovariance.mutable_pose_with_covariance()->mutable_pose()-> + mutable_orientation(), pose.Rot()); + // Copy twist from odometry msg. msgCovariance.mutable_twist_with_covariance()-> mutable_twist()->mutable_angular()->set_x(msg.twist().angular().x()); diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index e8b5bd7327..27fb5378f9 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -93,6 +93,7 @@ #include "gz/sim/Util.hh" // Components +#include "gz/sim/components/Actor.hh" #include "gz/sim/components/AngularAcceleration.hh" #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/AngularVelocityCmd.hh" @@ -622,6 +623,12 @@ class gz::sim::systems::PhysicsPrivate MinimumFeatureList, physics::sdf::ConstructSdfNestedModel>{}; + ////////////////////////////////////////////////// + // World models (used for world joints) + public: struct WorldModelFeatureList : physics::FeatureList< + MinimumFeatureList, + physics::WorldModelFeature>{}; + ////////////////////////////////////////////////// /// \brief World EntityFeatureMap public: using WorldEntityMap = EntityFeatureMap3d< @@ -632,7 +639,9 @@ class gz::sim::systems::PhysicsPrivate SetContactPropertiesCallbackFeatureList, NestedModelFeatureList, CollisionDetectorFeatureList, - SolverFeatureList>; + SolverFeatureList, + WorldModelFeatureList + >; /// \brief A map between world entity ids in the ECM to World Entities in /// gz-physics. @@ -1019,6 +1028,15 @@ void PhysicsPrivate::CreateWorldEntities(const EntityComponentManager &_ecm, } } + // World Model proxy (used for joints directly under in SDF) + auto worldModelFeature = + this->entityWorldMap.EntityCast(_entity); + if (worldModelFeature) + { + auto modelPtrPhys = worldModelFeature->GetWorldModel(); + this->entityModelMap.AddEntity(_entity, modelPtrPhys); + } + return true; }); } @@ -2857,8 +2875,14 @@ std::map PhysicsPrivate::ChangedLinks( if (this->linkAddedToModel.find(_entity) == this->linkAddedToModel.end()) { - gzerr << "Internal error: link [" << _entity - << "] not in entity map" << std::endl; + // ignore links from actors for now + auto parentId = + _ecm.Component(_entity)->Data(); + if (!_ecm.Component(parentId)) + { + gzerr << "Internal error: link [" << _entity + << "] not in entity map" << std::endl; + } } return true; } @@ -3313,6 +3337,53 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); + // body linear velocity + _ecm.Each( + [&](const Entity&, const components::Pose *_pose, + components::LinearVelocity *_linearVel, + const components::ParentEntity *_parent)->bool + { + // check if parent entity is a link, e.g. entity is sensor / collision + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); + math::Vector3d entityWorldLinearVel = + math::eigen3::convert(entityFrameData.linearVelocity); + + auto entityBodyLinearVel = + entityWorldPose.Rot().RotateVectorReverse(entityWorldLinearVel); + *_linearVel = components::LinearVelocity(entityBodyLinearVel); + } + + return true; + }); + + // world angular velocity + _ecm.Each( + [&](const Entity&, + const components::Pose *_pose, + components::WorldAngularVelocity *_worldAngularVel, + const components::ParentEntity *_parent)->bool + { + // check if parent entity is a link, e.g. entity is sensor / collision + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + // set entity world angular velocity + *_worldAngularVel = components::WorldAngularVelocity( + math::eigen3::convert(entityFrameData.angularVelocity)); + } + + return true; + }); + // body angular velocity _ecm.Each( @@ -3339,6 +3410,27 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, return true; }); + // world linear acceleration + _ecm.Each( + [&](const Entity &, + const components::Pose *_pose, + components::WorldLinearAcceleration *_worldLinearAcc, + const components::ParentEntity *_parent)->bool + { + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + // set entity world linear acceleration + *_worldLinearAcc = components::WorldLinearAcceleration( + math::eigen3::convert(entityFrameData.linearAcceleration)); + } + + return true; + }); + // body linear acceleration _ecm.Each( @@ -3361,6 +3453,52 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, *_linearAcc = components::LinearAcceleration(entityBodyLinearAcc); } + return true; + }); + + // world angular acceleration + _ecm.Each( + [&](const Entity &, + const components::Pose *_pose, + components::WorldAngularAcceleration *_worldAngularAcc, + const components::ParentEntity *_parent)->bool + { + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + // set entity world angular acceleration + *_worldAngularAcc = components::WorldAngularAcceleration( + math::eigen3::convert(entityFrameData.angularAcceleration)); + } + + return true; + }); + + // body angular acceleration + _ecm.Each( + [&](const Entity &, + const components::Pose *_pose, + components::AngularAcceleration *_angularAcc, + const components::ParentEntity *_parent)->bool + { + if (auto linkPhys = this->entityLinkMap.Get(_parent->Data())) + { + const auto entityFrameData = + this->LinkFrameDataAtOffset(linkPhys, _pose->Data()); + + auto entityWorldPose = math::eigen3::convert(entityFrameData.pose); + math::Vector3d entityWorldAngularAcc = + math::eigen3::convert(entityFrameData.angularAcceleration); + + auto entityBodyAngularAcc = + entityWorldPose.Rot().RotateVectorReverse(entityWorldAngularAcc); + *_angularAcc = components::AngularAcceleration(entityBodyAngularAcc); + } + return true; }); GZ_PROFILE_END(); @@ -3551,7 +3689,8 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, // TODO(louise) Skip this if there are no collision features this->UpdateCollisions(_ecm); -} +} // NOLINT readability/fn_size +// TODO (azeey) Reduce size of function and remove the NOLINT above ////////////////////////////////////////////////// void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 30c10014c7..6cf0b704d2 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -32,6 +32,7 @@ #include #include "gz/sim/components/AirPressureSensor.hh" +#include "gz/sim/components/AirSpeedSensor.hh" #include "gz/sim/components/Altimeter.hh" #include "gz/sim/components/Camera.hh" #include "gz/sim/components/CastShadows.hh" @@ -52,6 +53,7 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/ParticleEmitter.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/RgbdCamera.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/Sensor.hh" @@ -167,6 +169,15 @@ class gz::sim::systems::SceneBroadcasterPrivate public: static void AddParticleEmitters(msgs::Link *_msg, const Entity _entity, const SceneGraphType &_graph); + /// \brief Adds projectors to a msgs::Link object based on the + /// contents of the scene graph + /// \param[inout] _msg Pointer to msg object to which the projectors + /// will be added. + /// \param[in] _entity Parent entity in the graph + /// \param[in] _graph Scene graph + public: static void AddProjectors(msgs::Link *_msg, + const Entity _entity, const SceneGraphType &_graph); + /// \brief Recursively remove entities from the graph /// \param[in] _entity Entity /// \param[in/out] _graph Scene graph @@ -893,6 +904,12 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( { sensorMsg->set_type("air_pressure"); } + auto airSpeedComp = _manager.Component< + components::AirSpeedSensor>(_entity); + if (airSpeedComp) + { + sensorMsg->set_type("air_speed"); + } auto cameraComp = _manager.Component(_entity); if (cameraComp) { @@ -1072,6 +1089,29 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( return true; }); + // Projectors + _manager.EachNew( + [&](const Entity &_entity, + const components::Projector *_projectorComp, + const components::ParentEntity *_parentComp, + const components::Pose *_poseComp) -> bool + { + auto projectorMsg = std::make_shared(); + projectorMsg->CopyFrom( + convert(_projectorComp->Data())); + // \todo(anyone) add id field to projector msg + // projectorMsg->set_id(_entity); + projectorMsg->mutable_pose()->CopyFrom( + msgs::Convert(_poseComp->Data())); + + // Add to graph + newGraph.AddVertex(projectorMsg->name(), projectorMsg, _entity); + newGraph.AddEdge({_parentComp->Data(), _entity}, true); + newEntity = true; + return true; + }); + // Update the whole scene graph from the new graph { std::lock_guard lock(this->graphMutex); @@ -1253,6 +1293,24 @@ void SceneBroadcasterPrivate::AddParticleEmitters(msgs::Link *_msg, } } +////////////////////////////////////////////////// +void SceneBroadcasterPrivate::AddProjectors(msgs::Link *_msg, + const Entity _entity, const SceneGraphType &_graph) +{ + if (!_msg) + return; + + for (const auto &vertex : _graph.AdjacentsFrom(_entity)) + { + auto projectorMsg = std::dynamic_pointer_cast( + vertex.second.get().Data()); + if (!projectorMsg) + continue; + + _msg->add_projector()->CopyFrom(*projectorMsg); + } +} + ////////////////////////////////////////////////// void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, const SceneGraphType &_graph) @@ -1281,6 +1339,9 @@ void SceneBroadcasterPrivate::AddLinks(msgs::Model *_msg, const Entity _entity, // Particle emitters AddParticleEmitters(msgOut, vertex.second.get().Id(), _graph); + + // Projectors + AddProjectors(msgOut, vertex.second.get().Id(), _graph); } } diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index ff5fc17182..a30bcf319e 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -17,7 +17,6 @@ #include "Sensors.hh" -#include #include #include #include @@ -76,7 +75,7 @@ class gz::sim::systems::SensorsPrivate public: sensors::Manager sensorManager; /// \brief used to store whether rendering objects have been created. - public: bool initialized = false; + public: std::atomic initialized { false }; /// \brief Main rendering interface public: RenderUtil renderUtil; @@ -99,7 +98,7 @@ class gz::sim::systems::SensorsPrivate /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera - public: std::map cameras; + public: std::unordered_map cameras; /// \brief Maps gazebo entity to its matching sensor ID /// @@ -114,7 +113,7 @@ class gz::sim::systems::SensorsPrivate public: bool doInit { false }; /// \brief Flag to signal if rendering update is needed - public: bool updateAvailable { false }; + public: std::atomic updateAvailable { false }; /// \brief Flag to signal if a rendering update must be done public: std::atomic forceUpdate { false }; @@ -141,15 +140,17 @@ class gz::sim::systems::SensorsPrivate /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; + /// \brief Next sensors update time + public: std::chrono::steady_clock::duration nextUpdateTime; + /// \brief Sensors to include in the next rendering iteration - public: std::vector activeSensors; + public: std::set activeSensors; - /// \brief Mutex to protect sensorMask - public: std::mutex sensorMaskMutex; + /// \brief Sensors to be updated next + public: std::set sensorsToUpdate; - /// \brief Mask sensor updates for sensors currently being rendered - public: std::map sensorMask; + /// \brief Mutex to protect sensorMask + public: std::mutex sensorsMutex; /// \brief Pointer to the event manager public: EventManager *eventManager{nullptr}; @@ -201,6 +202,14 @@ class gz::sim::systems::SensorsPrivate /// \param[in] _ecm Entity component manager public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Get the next closest sensor update time + public: std::chrono::steady_clock::duration NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime); + + /// \brief Check if any of the sensors have connections + public: bool SensorsHaveConnections(); + /// \brief Use to optionally set the background color. public: std::optional backgroundColor; @@ -217,7 +226,7 @@ class gz::sim::systems::SensorsPrivate public: std::unordered_map modelBatteryStateChanged; /// \brief A map of sensor ids to their active state - public: std::map sensorStateChanged; + public: std::unordered_map sensorStateChanged; /// \brief Disable sensors if parent model's battery is drained /// Affects sensors that are in nested models @@ -250,6 +259,7 @@ void SensorsPrivate::WaitForInit() if (this->ambientLight) this->renderUtil.SetAmbientLight(*this->ambientLight); #ifndef __APPLE__ + this->renderUtil.Init(); #else // On macOS the render engine must be initialised on the main thread. @@ -269,11 +279,13 @@ void SensorsPrivate::WaitForInit() ////////////////////////////////////////////////// void SensorsPrivate::RunOnce() { - std::unique_lock lock(this->renderMutex); - this->renderCv.wait(lock, [this]() { - return !this->running || this->updateAvailable; - }); + std::unique_lock cvLock(this->renderMutex); + this->renderCv.wait(cvLock, [this]() + { + return !this->running || this->updateAvailable; + }); + } if (!this->running) return; @@ -281,13 +293,22 @@ void SensorsPrivate::RunOnce() if (!this->scene) return; + std::chrono::steady_clock::duration updateTimeApplied; GZ_PROFILE("SensorsPrivate::RunOnce"); { GZ_PROFILE("Update"); + std::unique_lock timeLock(this->renderMutex); this->renderUtil.Update(); + updateTimeApplied = this->updateTime; + } + + bool activeSensorsEmpty = true; + { + std::unique_lock lk(this->sensorsMutex); + activeSensorsEmpty = this->activeSensors.empty(); } - if (!this->activeSensors.empty() || this->forceUpdate) + if (!activeSensorsEmpty || this->forceUpdate) { // disable sensors that are out of battery or re-enable sensors that are // being charged @@ -306,30 +327,10 @@ void SensorsPrivate::RunOnce() this->sensorStateChanged.clear(); } - // Check the active sensors against masked sensors. - // - // The internal state of a rendering sensor is not updated until the - // rendering operation is complete, which can leave us in a position - // where the sensor is falsely indicating that an update is needed. - // - // To prevent this, add sensors that are currently being rendered to - // a mask. Sensors are removed from the mask when 90% of the update - // delta has passed, which will allow rendering to proceed. - { - std::unique_lock lockMask(this->sensorMaskMutex); - for (const auto & sensor : this->activeSensors) - { - // 90% of update delta (1/UpdateRate()); - auto delta = std::chrono::duration_cast( - std::chrono::duration< double >(0.9 / sensor->UpdateRate())); - this->sensorMask[sensor->Id()] = this->updateTime + delta; - } - } - { GZ_PROFILE("PreRender"); this->eventManager->Emit(); - this->scene->SetTime(this->updateTime); + this->scene->SetTime(updateTimeApplied); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true @@ -340,7 +341,7 @@ void SensorsPrivate::RunOnce() // disable sensors that have no subscribers to prevent doing unnecessary // work std::unordered_set tmpDisabledSensors; - this->sensorMaskMutex.lock(); + this->sensorsMutex.lock(); for (auto id : this->sensorIds) { sensors::Sensor *s = this->sensorManager.Sensor(id); @@ -351,12 +352,16 @@ void SensorsPrivate::RunOnce() tmpDisabledSensors.insert(rs); } } - this->sensorMaskMutex.unlock(); + this->sensorsMutex.unlock(); + // safety check to see if reset occurred while we're rendering + // avoid publishing outdated data if reset occurred + std::unique_lock timeLock(this->renderMutex); + if (updateTimeApplied <= this->updateTime) { // publish data GZ_PROFILE("RunOnce"); - this->sensorManager.RunOnce(this->updateTime); + this->sensorManager.RunOnce(updateTimeApplied); this->eventManager->Emit(); } @@ -376,12 +381,12 @@ void SensorsPrivate::RunOnce() this->eventManager->Emit(); } + std::unique_lock lk(this->sensorsMutex); this->activeSensors.clear(); } this->updateAvailable = false; this->forceUpdate = false; - lock.unlock(); this->renderCv.notify_one(); } @@ -406,6 +411,7 @@ void SensorsPrivate::RenderThread() for (const auto id : this->sensorIds) this->sensorManager.Remove(id); + this->renderUtil.Destroy(); gzdbg << "SensorsPrivate::RenderThread stopped" << std::endl; } @@ -455,15 +461,9 @@ void Sensors::RemoveSensor(const Entity &_entity) // Locking mutex to make sure the vector is not being changed while // the rendering thread is iterating over it { - std::unique_lock lock(this->dataPtr->sensorMaskMutex); - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(idIter->second); - auto rs = dynamic_cast(s); - auto activeSensorIt = std::find(this->dataPtr->activeSensors.begin(), - this->dataPtr->activeSensors.end(), rs); - if (activeSensorIt != this->dataPtr->activeSensors.end()) - { - this->dataPtr->activeSensors.erase(activeSensorIt); - } + std::unique_lock lock(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors.erase(idIter->second); + this->dataPtr->sensorsToUpdate.erase(idIter->second); } // update cameras list @@ -598,8 +598,9 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) gzdbg << "Resetting Sensors\n"; { - std::unique_lock lock(this->dataPtr->sensorMaskMutex); - this->dataPtr->sensorMask.clear(); + std::unique_lock lock(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors.clear(); + this->dataPtr->sensorsToUpdate.clear(); } for (auto id : this->dataPtr->sensorIds) @@ -614,6 +615,9 @@ void Sensors::Reset(const UpdateInfo &_info, EntityComponentManager &) s->SetNextDataUpdateTime(_info.simTime); } + this->dataPtr->nextUpdateTime = _info.simTime; + std::unique_lock lock(this->dataPtr->renderMutex); + this->dataPtr->updateTime = _info.simTime; } } @@ -622,7 +626,6 @@ void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { GZ_PROFILE("Sensors::Update"); - std::unique_lock lock(this->dataPtr->renderMutex); #ifdef __APPLE__ // On macOS the render engine must be initialised on the main thread. @@ -636,8 +639,10 @@ void Sensors::Update(const UpdateInfo &_info, _ecm.HasComponentType(components::SegmentationCamera::typeId) || _ecm.HasComponentType(components::WideAngleCamera::typeId))) { + std::unique_lock lock(this->dataPtr->renderMutex); igndbg << "Initialization needed" << std::endl; this->dataPtr->renderUtil.Init(); + this->dataPtr->nextUpdateTime = _info.simTime; } #endif @@ -652,9 +657,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { GZ_PROFILE("Sensors::PostUpdate"); - { - std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && (this->dataPtr->forceUpdate || _ecm.HasComponentType(components::BoundingBoxCamera::typeId) || @@ -666,6 +669,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::SegmentationCamera::typeId) || _ecm.HasComponentType(components::WideAngleCamera::typeId))) { + std::unique_lock lock(this->dataPtr->renderMutex); gzdbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); @@ -674,67 +678,70 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - - std::vector activeSensors; - { - std::unique_lock lk(this->dataPtr->sensorMaskMutex); - for (auto id : this->dataPtr->sensorIds) - { - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); - - if (nullptr == s) - { - continue; - } - - auto rs = dynamic_cast(s); - - if (nullptr == rs) - { - continue; - } - - auto it = this->dataPtr->sensorMask.find(id); - if (it != this->dataPtr->sensorMask.end()) - { - if (it->second <= _info.simTime) - { - this->dataPtr->sensorMask.erase(it); - } - else - { - continue; - } - } + std::unique_lock lock(this->dataPtr->renderMutex); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + this->dataPtr->updateTime = _info.simTime; + } - if (rs && rs->NextDataUpdateTime() <= _info.simTime) - { - activeSensors.push_back(rs); - } - } + // check connections to render events + // we will need to perform render updates if there are event subscribers + // \todo(anyone) This currently forces scene tree updates at the sim update + // rate which can be too frequent and causes a performance hit. + // We should look into throttling render updates + bool hasRenderConnections = + (this->dataPtr->eventManager->ConnectionCount() > 0u || + this->dataPtr->eventManager->ConnectionCount() > 0u || + this->dataPtr->eventManager->ConnectionCount() > 0u); + + // if nextUpdateTime is max, it probably means there are previously + // no active sensors or sensors with connections. + // In this case, check if sensors have connections now. If so, we need to + // set the nextUpdateTime + if (this->dataPtr->nextUpdateTime == + std::chrono::steady_clock::duration::max() && + this->dataPtr->SensorsHaveConnections()) + { + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); } - if (!activeSensors.empty() || + // notify the render thread if updates are available + if (hasRenderConnections || + this->dataPtr->nextUpdateTime <= _info.simTime || this->dataPtr->renderUtil.PendingSensors() > 0 || this->dataPtr->forceUpdate) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); - std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->renderCv.wait(lock, [this] { - return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); + { + std::unique_lock cvLock(this->dataPtr->renderMutex); + this->dataPtr->renderCv.wait(cvLock, [this] { + return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); + } if (!this->dataPtr->running) { return; } - this->dataPtr->activeSensors = std::move(activeSensors); - this->dataPtr->updateTime = _info.simTime; - this->dataPtr->updateAvailable = true; + { + std::unique_lock lockSensors(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors = + std::move(this->dataPtr->sensorsToUpdate); + + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); + + // Force scene tree update if there are sensors to be created or + // subscribes to the render events. This does not necessary force + // sensors to update. Only active sensors will be updated + this->dataPtr->forceUpdate = + (this->dataPtr->renderUtil.PendingSensors() > 0) || + hasRenderConnections; + this->dataPtr->updateAvailable = true; + } this->dataPtr->renderCv.notify_one(); } } @@ -940,6 +947,86 @@ std::string Sensors::CreateSensor(const Entity &_entity, return sensor->Name(); } +////////////////////////////////////////////////// +std::chrono::steady_clock::duration SensorsPrivate::NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime) +{ + _sensorsToUpdate.clear(); + std::chrono::steady_clock::duration minNextUpdateTime = + std::chrono::steady_clock::duration::max(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (!rs->HasConnections()) + { + continue; + } + + std::chrono::steady_clock::duration time; + // if sensor's next update tims is less or equal to current sim time then + // it's in the process of being updated by the render loop + // Set their next update time to be current time + update period + if (rs->NextDataUpdateTime() <= _currentTime) + { + time = rs->NextDataUpdateTime() + + std::chrono::duration_cast( + std::chrono::duration(1.0 / rs->UpdateRate())); + } + else + { + time = rs->NextDataUpdateTime(); + } + + if (time <= minNextUpdateTime) + { + _sensorsToUpdate.clear(); + minNextUpdateTime = time; + } + _sensorsToUpdate.insert(id); + } + return minNextUpdateTime; +} + +////////////////////////////////////////////////// +bool SensorsPrivate::SensorsHaveConnections() +{ + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (rs->HasConnections()) + { + return true; + } + } + return false; +} + GZ_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemReset, diff --git a/src/systems/shader_param/ShaderParam.cc b/src/systems/shader_param/ShaderParam.cc index f9b054140b..453d9759b3 100644 --- a/src/systems/shader_param/ShaderParam.cc +++ b/src/systems/shader_param/ShaderParam.cc @@ -330,6 +330,15 @@ void ShaderParamPrivate::OnUpdate() } } + // inherit cast shadows property from existing material + rendering::MaterialPtr oldMat; + if (visual->GeometryCount() > 0u) + oldMat = this->visual->GeometryByIndex(0u)->Material(); + else + oldMat = this->visual->Material(); + if (oldMat) + mat->SetCastShadows(oldMat->CastShadows()); + this->visual->SetMaterial(mat); scene->DestroyMaterial(mat); this->material = this->visual->Material(); diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 4885acca82..b911a5674a 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -32,10 +32,12 @@ #include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/BatterySoC.hh" +#include "gz/sim/components/BatteryPowerLoad.hh" #include "gz/sim/components/ChildLinkName.hh" #include "gz/sim/components/JointAxis.hh" #include "gz/sim/components/JointVelocityCmd.hh" #include "gz/sim/components/LinearVelocity.hh" +#include "gz/sim/components/Name.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/World.hh" #include "gz/sim/Link.hh" @@ -78,6 +80,9 @@ class gz::sim::systems::ThrusterPrivateData /// \brief The link entity which will spin public: Entity linkEntity; + /// \brief Battery consumer entity + public: Entity consumerEntity; + /// \brief Axis along which the propeller spins. Expressed in the joint /// frame. Addume this doesn't change during simulation. public: math::Vector3d jointAxis; @@ -142,6 +147,15 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Topic name used to control thrust. Optional public: std::string topic = ""; + /// \brief Battery entity used by the thruster to consume power. + public: std::string batteryName = ""; + + /// \brief Battery power load of the thruster. + public: double powerLoad = 0.0; + + /// \brief Has the battery consumption being initialized. + public: bool batteryInitialized = false; + /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); @@ -176,14 +190,14 @@ Thruster::Thruster(): ///////////////////////////////////////////////// void Thruster::Configure( - const gz::sim::Entity &_entity, + const Entity &_entity, const std::shared_ptr &_sdf, - gz::sim::EntityComponentManager &_ecm, - gz::sim::EventManager &/*_eventMgr*/) + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) { // Create model object, to access convenient functions this->dataPtr->modelEntity = _entity; - auto model = gz::sim::Model(_entity); + auto model = Model(_entity); auto modelName = model.Name(_ecm); // Get namespace @@ -279,7 +293,7 @@ void Thruster::Configure( } this->dataPtr->jointAxis = - _ecm.Component( + _ecm.Component( this->dataPtr->jointEntity)->Data().Xyz(); this->dataPtr->jointPose = _ecm.Component( @@ -287,7 +301,7 @@ void Thruster::Configure( // Get link entity auto childLink = - _ecm.Component( + _ecm.Component( this->dataPtr->jointEntity); this->dataPtr->linkEntity = model.LinkByName(_ecm, childLink->Data()); @@ -431,6 +445,22 @@ void Thruster::Configure( { gzdbg << "Using velocity control for propeller joint." << std::endl; } + + // Get power load and battery name info + if (_sdf->HasElement("power_load")) + { + if (!_sdf->HasElement("battery_name")) + { + ignerr << "Specified a but missing ." + "Specify a battery name so the power load can be assigned to it." + << std::endl; + } + else + { + this->dataPtr->powerLoad = _sdf->Get("power_load"); + this->dataPtr->batteryName = _sdf->Get("battery_name"); + } + } } ///////////////////////////////////////////////// @@ -533,8 +563,57 @@ void Thruster::PreUpdate( return; } + // Init battery consumption if it was set + if (!this->dataPtr->batteryName.empty() && + !this->dataPtr->batteryInitialized) + { + this->dataPtr->batteryInitialized = true; + + // Check that a battery exists with the specified name + Entity batteryEntity; + int numBatteriesWithName = 0; + _ecm.Each( + [&](const Entity &_entity, + const components::BatterySoC */*_BatterySoC*/, + const components::Name *_name)->bool + { + if (this->dataPtr->batteryName == _name->Data()) + { + ++numBatteriesWithName; + batteryEntity = _entity; + } + return true; + }); + if (numBatteriesWithName == 0) + { + ignerr << "Can't assign battery consumption to battery: [" + << this->dataPtr->batteryName << "]. No batteries" + "were found with the given name." << std::endl; + return; + } + if (numBatteriesWithName > 1) + { + ignerr << "More than one battery found with name: [" + << this->dataPtr->batteryName << "]. Please make" + "sure battery names are unique within the system." + << std::endl; + return; + } + + // Create the battery consumer entity and its component + this->dataPtr->consumerEntity = _ecm.CreateEntity(); + components::BatteryPowerLoadInfo batteryPowerLoadInfo{ + batteryEntity, this->dataPtr->powerLoad}; + _ecm.CreateComponent(this->dataPtr->consumerEntity, + components::BatteryPowerLoad(batteryPowerLoadInfo)); + _ecm.SetParentEntity(this->dataPtr->consumerEntity, batteryEntity); + } + gz::sim::Link link(this->dataPtr->linkEntity); + + auto pose = worldPose(this->dataPtr->linkEntity, _ecm); + // TODO(arjo129): add logic for custom coordinate frame // Convert joint axis to the world frame const auto linkWorldPose = worldPose(this->dataPtr->linkEntity, _ecm); diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index ad12b73b11..bfb258f823 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -832,14 +832,7 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) return std::all_of(this->matchers.begin(), this->matchers.end(), [&](const auto &_matcher) { - try - { - return _matcher->Match(_inputMsg); - } catch (const google::protobuf::FatalException &err) - { - gzerr << err.what() << std::endl; - return false; - } + return _matcher->Match(_inputMsg); }); } diff --git a/test/benchmark/each.cc b/test/benchmark/each.cc index 1c8175b9b9..8e7bc58c81 100644 --- a/test/benchmark/each.cc +++ b/test/benchmark/each.cc @@ -423,7 +423,11 @@ BENCHMARK_REGISTER_F(ManyComponentFixture, Each10ComponentCache) ->Unit(benchmark::kMillisecond); // OSX needs the semicolon, Ubuntu complains that there's an extra ';' +#if !defined(_MSC_VER) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" +#endif BENCHMARK_MAIN(); +#if !defined(_MSC_VER) #pragma GCC diagnostic pop +#endif diff --git a/test/benchmark/ecm_serialize.cc b/test/benchmark/ecm_serialize.cc index 6d3e339587..2d976df4db 100644 --- a/test/benchmark/ecm_serialize.cc +++ b/test/benchmark/ecm_serialize.cc @@ -93,8 +93,8 @@ void BM_Serialize1Component(benchmark::State &_st) serializedSize = stateMsg.ByteSize(); #endif } - _st.counters["serialized_size"] = serializedSize; - _st.counters["num_entities"] = entityCount; + _st.counters["serialized_size"] = static_cast(serializedSize); + _st.counters["num_entities"] = static_cast(entityCount); _st.counters["num_components"] = 1; } @@ -125,8 +125,8 @@ void BM_Serialize5Component(benchmark::State &_st) serializedSize = stateMsg.ByteSize(); #endif } - _st.counters["serialized_size"] = serializedSize; - _st.counters["num_entities"] = entityCount; + _st.counters["serialized_size"] = static_cast(serializedSize); + _st.counters["num_entities"] = static_cast(entityCount); _st.counters["num_components"] = 5; } @@ -149,7 +149,11 @@ BENCHMARK(BM_Serialize5Component) ->Unit(benchmark::kMillisecond); // OSX needs the semicolon, Ubuntu complains that there's an extra ';' +#if !defined(_MSC_VER) #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" +#endif BENCHMARK_MAIN(); +#if !defined(_MSC_VER) #pragma GCC diagnostic pop +#endif diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2629f565b4..621d9150ce 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -2,9 +2,12 @@ set(TEST_TYPE "INTEGRATION") set(tests ackermann_steering_system.cc + actor.cc acoustic_comms.cc air_pressure_system.cc + air_speed_system.cc altimeter_system.cc + added_mass.cc apply_joint_force_system.cc apply_link_wrench_system.cc battery_plugin.cc @@ -30,6 +33,8 @@ set(tests hydrodynamics.cc hydrodynamics_flags.cc imu_system.cc + include_sim_hh.cc + joint.cc joint_controller_system.cc joint_position_controller_system.cc joint_state_publisher_system.cc @@ -38,6 +43,7 @@ set(tests lift_drag_system.cc level_manager.cc level_manager_runtime_performers.cc + light.cc link.cc logical_camera_system.cc logical_audio_sensor_plugin.cc @@ -57,6 +63,7 @@ set(tests physics_system.cc play_pause.cc pose_publisher_system.cc + projector.cc rf_comms.cc recreate_entities.cc reset.cc @@ -65,6 +72,7 @@ set(tests scene_broadcaster_system.cc sdf_frame_semantics.cc sdf_include.cc + sensor.cc spherical_coordinates.cc thruster.cc touch_plugin.cc @@ -86,6 +94,7 @@ endif() # Tests that require a valid display set(tests_needing_display + actor_trajectory.cc camera_lens_flare.cc camera_sensor_background.cc camera_sensor_background_from_scene.cc @@ -101,6 +110,7 @@ set(tests_needing_display rgbd_camera.cc sensors_system.cc sensors_system_battery.cc + sensors_system_update_rate.cc shader_param_system.cc thermal_sensor_system.cc thermal_system.cc @@ -214,4 +224,7 @@ if(VALID_DISPLAY AND VALID_DRI_DISPLAY AND TARGET INTEGRATION_sensors_system) target_link_libraries(INTEGRATION_sensors_system gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} ) + target_link_libraries(INTEGRATION_actor_trajectory + gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} + ) endif() diff --git a/test/integration/ModelPhotoShootTest.hh b/test/integration/ModelPhotoShootTest.hh index 2202ab9397..5636036267 100644 --- a/test/integration/ModelPhotoShootTest.hh +++ b/test/integration/ModelPhotoShootTest.hh @@ -301,6 +301,8 @@ class ModelPhotoShootTest : public InternalFixture<::testing::Test> testImages("3.png", "3_test.png"); testImages("4.png", "4_test.png"); testImages("5.png", "5_test.png"); + + postRenderConn.reset(); } private: bool takeTestPics{false}; diff --git a/test/integration/ackermann_steering_system.cc b/test/integration/ackermann_steering_system.cc index 75e6488527..5953ac6ea9 100644 --- a/test/integration/ackermann_steering_system.cc +++ b/test/integration/ackermann_steering_system.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2021 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,13 +19,20 @@ #include #include #include +#include +#include #include #include +#include #include +#include +#include #include #include +#include "gz/sim/components/Joint.hh" +#include "gz/sim/components/JointPosition.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Pose.hh" @@ -41,6 +49,12 @@ using namespace gz; using namespace sim; using namespace std::chrono_literals; +/// \brief Test AckermannSteeringOnly system +class AckermannSteeringOnlyTest + : public InternalFixture<::testing::Test> +{ +}; + /// \brief Test AckermannSteering system class AckermannSteeringTest : public InternalFixture<::testing::TestWithParam> @@ -543,6 +557,86 @@ TEST_P(AckermannSteeringTest, EXPECT_EQ(5u, odomPosesCount); } +TEST_F(AckermannSteeringOnlyTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(SteerPublishCmd)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "ackermann_steering_only.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model/vehicle/steer_angle" + ); + + msgs::Double msg; + const double targetAngle{0.25}; + msg.set_data(targetAngle); + pub.Publish(msg); +} + +TEST_F(AckermannSteeringOnlyTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(SteerPublishActuatorsCmd)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "ackermann_steering_only_actuators.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Publish actuator command + transport::Node node; + auto pub = node.Advertise( + "/actuators"); + + const double targetAngle{0.25}; + msgs::Actuators msg; + msg.add_position(targetAngle); + + pub.Publish(msg); +} + +///////////////////////////////////////////////// +TEST_F(AckermannSteeringOnlyTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32( + PublishCmdCustomSubTopicsSteer)) +{ + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "ackermann_steering_only_custom_sub_topics.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model/vehicle/steerangle" + ); + + msgs::Double msg; + const double targetAngle{0.25}; + msg.set_data(targetAngle); + pub.Publish(msg); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, AckermannSteeringTest, ::testing::Range(1, 2)); diff --git a/test/integration/actor.cc b/test/integration/actor.cc new file mode 100644 index 0000000000..9540c6a5ca --- /dev/null +++ b/test/integration/actor.cc @@ -0,0 +1,282 @@ +/* + * 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 +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class ActorIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Actor actor; + EXPECT_FALSE(actor.Valid(ecm)); + } + + // Missing actor component + { + auto id = ecm.CreateEntity(); + Actor actor(id); + EXPECT_FALSE(actor.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + EXPECT_TRUE(actor.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No name + EXPECT_EQ(std::nullopt, actor.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("actor_name")); + EXPECT_EQ("actor_name", actor.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No pose + EXPECT_EQ(std::nullopt, actor.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, actor.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, WorldPose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No pose + EXPECT_EQ(std::nullopt, actor.WorldPose(ecm)); + + // Add world pose + math::Pose3d pose(0, 3, 9, 0.0, 1.2, -3.0); + ecm.CreateComponent(id, + components::WorldPose(pose)); + EXPECT_EQ(pose, actor.WorldPose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, TrajectoryPose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Actor()); + + Actor actor(id); + + // No pose + EXPECT_EQ(std::nullopt, actor.TrajectoryPose(ecm)); + + // Add pose + math::Pose3d pose(3, 42, 35, 2.1, 3.2, 1.3); + ecm.CreateComponent(id, + components::TrajectoryPose(pose)); + EXPECT_EQ(pose, actor.TrajectoryPose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, SetTrajectoryPose) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // No TrajectoryPose should exist by default + EXPECT_EQ(nullptr, ecm.Component(eActor)); + + math::Pose3d pose(0.1, 2.3, 4.7, 0, 0, 1.0); + actor.SetTrajectoryPose(ecm, pose); + + // trajectory pose should exist + EXPECT_NE(nullptr, ecm.Component(eActor)); + EXPECT_EQ(pose, + ecm.Component(eActor)->Data()); + + // Make sure the trajectory pose is updated + math::Pose3d pose2(1.0, 3.2, 7.4, 1.0, 0, 0.0); + actor.SetTrajectoryPose(ecm, pose2); + EXPECT_EQ(pose2, + ecm.Component(eActor)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, SetAnimationName) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // No AnimationName should exist by default + EXPECT_EQ(nullptr, ecm.Component(eActor)); + + std::string animName = "animation_name"; + actor.SetAnimationName(ecm, animName); + + // animation name should exist + EXPECT_NE(nullptr, ecm.Component(eActor)); + EXPECT_EQ(animName, + ecm.Component(eActor)->Data()); + + // Make sure the animation name is updated + std::string animName2 = "animation_name2"; + actor.SetAnimationName(ecm, animName2); + EXPECT_EQ(animName2, + ecm.Component(eActor)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, SetAnimationTime) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // No AnimationTime should exist by default + EXPECT_EQ(nullptr, ecm.Component(eActor)); + + std::chrono::steady_clock::duration animTime = std::chrono::milliseconds(30); + actor.SetAnimationTime(ecm, animTime); + + // animation time should exist + EXPECT_NE(nullptr, ecm.Component(eActor)); + EXPECT_EQ(animTime, + ecm.Component(eActor)->Data()); + + // Make sure the animation time is updated + std::chrono::steady_clock::duration animTime2 = std::chrono::milliseconds(70); + actor.SetAnimationTime(ecm, animTime2); + EXPECT_EQ(animTime2, + ecm.Component(eActor)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, AnimationName) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // animation name should return nullopt by default + EXPECT_EQ(std::nullopt, actor.AnimationName(ecm)); + + // get animation name + std::string animName = "animation_name"; + ecm.SetComponentData(eActor, animName); + EXPECT_EQ(animName, actor.AnimationName(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(ActorIntegrationTest, AnimationTime) +{ + EntityComponentManager ecm; + + auto eActor = ecm.CreateEntity(); + ecm.CreateComponent(eActor, components::Actor()); + + Actor actor(eActor); + EXPECT_EQ(eActor, actor.Entity()); + + ASSERT_TRUE(actor.Valid(ecm)); + + // animation time should return nullopt by default + EXPECT_EQ(std::nullopt, actor.AnimationTime(ecm)); + + // get animation time + std::chrono::steady_clock::duration animTime = std::chrono::milliseconds(60); + ecm.CreateComponent(eActor, + components::AnimationTime(animTime)); + EXPECT_EQ(animTime, actor.AnimationTime(ecm)); +} diff --git a/test/integration/actor_trajectory.cc b/test/integration/actor_trajectory.cc new file mode 100644 index 0000000000..8116499686 --- /dev/null +++ b/test/integration/actor_trajectory.cc @@ -0,0 +1,166 @@ +/* + * 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 "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" +#include "test_config.hh" + +#include "gz/sim/rendering/Events.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace std::chrono_literals; + +// Pointer to scene +rendering::ScenePtr g_scene; + +// Map of model names to their poses +std::unordered_map> g_modelPoses; + +// mutex to project model poses +std::mutex g_mutex; + +///////////////////////////////////////////////// +void OnPostRender() +{ + if (!g_scene) + { + g_scene = rendering::sceneFromFirstRenderEngine(); + } + ASSERT_TRUE(g_scene); + + auto rootVis = g_scene->RootVisual(); + ASSERT_TRUE(rootVis); + + // store all the model poses + std::lock_guard lock(g_mutex); + for (unsigned int i = 0; i < rootVis->ChildCount(); ++i) + { + auto vis = rootVis->ChildByIndex(i); + ASSERT_TRUE(vis); + g_modelPoses[vis->Name()].push_back(vis->WorldPose()); + } +} + +////////////////////////////////////////////////// +class ActorFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("gz::sim::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: sim::SystemPluginPtr systemPtr; + public: sim::MockSystem *mockSystem; + + private: sim::SystemLoader sm; +}; + +///////////////////////////////////////////////// +// Load the actor_trajectory.sdf world that animates a box (actor) to follow +// a trajectory. Verify that the box pose changes over time on the rendering +// side. +TEST_F(ActorFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(ActorTrajectoryNoMesh)) +{ + sim::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/actor_trajectory.sdf"; + + serverConfig.SetSdfFile(sdfFile); + sim::Server server(serverConfig); + + common::ConnectionPtr postRenderConn; + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + this->mockSystem->preUpdateCallback = + [&ecm](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + { + ecm = &_ecm; + }; + this->mockSystem->configureCallback = + [&](const sim::Entity &, + const std::shared_ptr &, + sim::EntityComponentManager &, + sim::EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&::OnPostRender)); + }; + + server.AddSystem(this->systemPtr); + server.Run(true, 500, false); + ASSERT_NE(nullptr, ecm); + + // verify that pose of the animated box exists + bool hasBoxPose = false; + int sleep = 0; + int maxSleep = 50; + const std::string boxName = "animated_box"; + unsigned int boxPoseCount = 0u; + while (!hasBoxPose && sleep++ < maxSleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + std::lock_guard lock(g_mutex); + if (g_modelPoses.find(boxName) != g_modelPoses.end()) + { + hasBoxPose = true; + boxPoseCount = g_modelPoses.size(); + } + } + EXPECT_TRUE(hasBoxPose); + EXPECT_LT(1u, boxPoseCount); + + // check that box is animated, i.e. pose changes over time + { + std::lock_guard lock(g_mutex); + auto it = g_modelPoses.find(boxName); + auto &poses = it->second; + for (unsigned int i = 0; i < poses.size()-1; ++i) + { + EXPECT_NE(poses[i], poses[i+1]); + } + } + + g_scene.reset(); +} diff --git a/test/integration/added_mass.cc b/test/integration/added_mass.cc new file mode 100644 index 0000000000..a751b10bba --- /dev/null +++ b/test/integration/added_mass.cc @@ -0,0 +1,355 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#define _USE_MATH_DEFINES +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "../helpers/EnvTestFixture.hh" + +// World file path. +const char *kWorldFilePath{"/test/worlds/added_mass_ellipsoids.sdf"}; + +// Names of the models. +const std::vector kModelNames = {"body1", "body2", "body3"}; + +// Link name (all models use the same name). +const char* kLinkName = "link"; + +// Update rate of the server. +const double kRate = 1000; + +// Force excitation amplitude and direction. +const double kForceVec[3] = {2000, 2000, 0}; + +// Force excitation angular velocity [rad / s]. +const double kForceAngVel = 3 * M_PI; + +// Torque excitation amplitude and direction. +const double kTorqueVec[3] = {200, 200, 0}; + +// Torque excitation angular velocity [rad / s]. +const double kTorqueAngVel = 2 * M_PI; + +// Total duration of the motion in iterations. +const uint64_t kIter = 1000; + +// Tolerances. +/* \TODO(mjcarroll) These tolerances are completely suitable for Jammy, + but are too tight on focal. +const struct {double pos, angle, axis, lin_vel, ang_vel;} kTols = { + 1e-3, // pos + 1e-5, // angle + 1e-4, // axis + 1e-5, // lin_vel + 1e-4 // ang_vel +}; +*/ + +const struct {double pos, angle, axis, lin_vel, ang_vel;} kTols = { + 1e-3, // pos + 1e-5, // angle + 1e-4, // axis + 2e-5, // lin_vel + 1e-4 // ang_vel +}; + +// Struct to store model/link state. +struct BodyState { + gz::math::Vector3d pos; + double angle; + gz::math::Vector3d axis; + gz::math::Vector3d lin_vel; + gz::math::Vector3d ang_vel; +}; + +// Mapping from model name to expected state. +const std::map kExpectedStates = +{ + { + "body1", + { + // pos + gz::math::Vector3d( + 0.1615036362557803, + 0.10710173528687592, + -0.0024940422102210013 + ), + // angle + 0.31351100787675423, + // axis + gz::math::Vector3d( + 0.9487133476959254, + 0.2967465880246766, + -0.10901580802481375 + ), + // lin_vel + gz::math::Vector3d( + 0.32087488227940963, + 0.212772641112832, + -0.010283659814296216 + ), + // ang_vel + gz::math::Vector3d( + 0.0031500380024329826, + 0.0022451901112414984, + -0.052564274312245286 + ) + } + }, + { + "body2", + { + // pos + gz::math::Vector3d( + 0.19339397146633158, + 0.18238590690754014, + -0.0006146904921505786 + ), + // angle + 0.3185857744101227, + // axis + gz::math::Vector3d( + 0.9309057658841812, + 0.3615623345327693, + -0.051837566405069514 + ), + // lin_vel + gz::math::Vector3d( + 0.386607475883442, + 0.36477533916100213, + -0.002544369239292084 + ), + // ang_vel + gz::math::Vector3d( + 0.0005006537195657914, + 0.0007044864197475587, + -0.012889587224151892 + ) + } + }, + { + "body3", + { + // pos + gz::math::Vector3d( + 0.0616809222579082, + 0.020951194425796703, + -0.0006836779301496875 + ), + // angle + 0.2990323477528498, + // axis + gz::math::Vector3d( + 0.992866493007573, + 0.11317163977508175, + -0.03752741682373132 + ), + // lin_vel + gz::math::Vector3d( + 0.12296544056644206, + 0.04157265120194949, + -0.002832810385198371 + ), + // ang_vel + gz::math::Vector3d( + 0.0017159692398096078, + 0.00022402938638815302, + -0.014274584996716126 + ) + } + }, +}; + +// Plugin that applies a sinusoidal wrench. +class SinusoidalWrenchPlugin: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPreUpdate +{ + public: void Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr + ) override; + + public: void PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm + ) override; + + public: gz::sim::EntityComponentManager *ecm{nullptr}; + + public: std::map link_entities; +}; + +// Store a reference to the ECM and to the link entities. +// +// The reference to the ECM is stored so it can be retrieved after the server +// has run. +void SinusoidalWrenchPlugin::Configure( + const gz::sim::Entity &, + const std::shared_ptr &, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager & +) +{ + this->ecm = &_ecm; + + for (std::string model_name : kModelNames) { + gz::sim::Entity model_entity = _ecm.EntityByComponents( + gz::sim::components::Name(model_name), + gz::sim::components::Model() + ); + ASSERT_NE(model_entity, gz::sim::kNullEntity); + + gz::sim::Model model = gz::sim::Model(model_entity); + ASSERT_TRUE(model.Valid(_ecm)); + + this->link_entities[model_name] = model.LinkByName(_ecm, kLinkName); + ASSERT_NE(this->link_entities[model_name], gz::sim::kNullEntity); + _ecm.CreateComponent( + this->link_entities[model_name], + gz::sim::components::WorldPose() + ); + + gz::sim::Link link = gz::sim::Link(this->link_entities[model_name]); + ASSERT_TRUE(link.Valid(_ecm)); + link.EnableVelocityChecks(_ecm); + } +} + +// Apply sinusoidal wrench before integration. +void SinusoidalWrenchPlugin::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm +) +{ + for (std::string model_name : kModelNames) { + gz::sim::Link link = gz::sim::Link(this->link_entities[model_name]); + ASSERT_TRUE(link.Valid(_ecm)); + + // Get time in seconds. + double t = std::chrono::duration(_info.simTime).count(); + + gz::math::Vector3d force( + kForceVec[0] * std::sin(kForceAngVel * t), + kForceVec[1] * std::sin(kForceAngVel * t), + kForceVec[2] * std::sin(kForceAngVel * t) + ); + gz::math::Vector3d torque( + kTorqueVec[0] * std::sin(kTorqueAngVel * t), + kTorqueVec[1] * std::sin(kTorqueAngVel * t), + kTorqueVec[2] * std::sin(kTorqueAngVel * t) + ); + + link.AddWorldWrench(_ecm, force, torque); + } +} + +class EmptyTestFixture: public InternalFixture<::testing::Test> {}; + +// Check that the link state at the end of the motion matches the expected +// state. +TEST_F(EmptyTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(MotionTest)) +{ + // Start server and run. + gz::sim::ServerConfig serverConfig; + serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, kWorldFilePath) + ); + serverConfig.SetUpdateRate(kRate); + gz::sim::Server server = gz::sim::Server(serverConfig); + auto system = std::make_shared(); + server.AddSystem(system); + ASSERT_FALSE(server.Running()); + ASSERT_TRUE(server.Run(true, kIter, false)); + + gz::sim::EntityComponentManager *ecm = system->ecm; + + for (std::string model_name : kModelNames) { + gz::sim::Link link = gz::sim::Link(system->link_entities[model_name]); + ASSERT_TRUE(link.Valid(*ecm)); + + // Check pose. + const std::optional maybe_pose = link.WorldPose(*ecm); + EXPECT_TRUE(maybe_pose); + if (maybe_pose) { + const gz::math::Pose3d pose = maybe_pose.value(); + const std::optional maybe_pos = pose.Pos(); + EXPECT_TRUE(maybe_pos); + if (maybe_pos) { + gz::math::Vector3d pos = maybe_pos.value(); + EXPECT_LE( + (pos - kExpectedStates.at(model_name).pos).Length(), + kTols.pos + ); + } + + const gz::math::Quaternion quat = pose.Rot(); + gz::math::Vector3d axis; + double angle = 0; + quat.AxisAngle(axis, angle); + EXPECT_LE( + (axis - kExpectedStates.at(model_name).axis).Length(), + kTols.axis + ) << kExpectedStates.at(model_name).axis; + EXPECT_LE( + std::abs(angle - kExpectedStates.at(model_name).angle), + kTols.angle + ) << kExpectedStates.at(model_name).angle; + } + + // Check velocities. + std::optional maybe_lin_vel = + link.WorldLinearVelocity(*ecm); + EXPECT_TRUE(maybe_lin_vel); + if (maybe_lin_vel) { + gz::math::Vector3d lin_vel = maybe_lin_vel.value(); + EXPECT_LE( + (lin_vel - kExpectedStates.at(model_name).lin_vel).Length(), + kTols.lin_vel + ) << kExpectedStates.at(model_name).lin_vel; + } + + std::optional maybe_ang_vel = + link.WorldAngularVelocity(*ecm); + EXPECT_TRUE(maybe_ang_vel); + if (maybe_ang_vel) { + gz::math::Vector3d ang_vel = maybe_ang_vel.value(); + EXPECT_LE( + (ang_vel - kExpectedStates.at(model_name).ang_vel).Length(), + kTols.ang_vel + ) << kExpectedStates.at(model_name).ang_vel; + } + } +} diff --git a/test/integration/air_speed_system.cc b/test/integration/air_speed_system.cc new file mode 100644 index 0000000000..82680e1a04 --- /dev/null +++ b/test/integration/air_speed_system.cc @@ -0,0 +1,136 @@ +/* + * 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 "gz/sim/components/AirSpeedSensor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/Server.hh" +#include "test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test AirSpeedTest system +class AirSpeedTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(AirSpeedTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirSpeed)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "air_speed.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "air_speed_sensor"; + + auto topic = "world/air_speed_sensor/model/air_speed_model/link/link/" + "sensor/air_speed_sensor/air_speed"; + + bool updateChecked{false}; + + // Create a system that checks sensor topic + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, + const components::AirSpeedSensor *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + updateChecked = true; + + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // Subscribe to air_speed topic + bool received{false}; + msgs::AirSpeedSensor msg; + msg.Clear(); + std::function cb = + [&received, &msg](const msgs::AirSpeedSensor &_msg) + { + // Only need one message + if (received) + return; + + msg = _msg; + received = true; + }; + + transport::Node node; + node.Subscribe(topic, cb); + + // Run server + server.Run(true, 100, false); + EXPECT_TRUE(updateChecked); + + // Wait for message to be received + for (int sleep = 0; !received && sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(received); + + // check air pressure + EXPECT_TRUE(msg.has_header()); + EXPECT_TRUE(msg.header().has_stamp()); + EXPECT_EQ(0, msg.header().stamp().sec()); + EXPECT_LT(0, msg.header().stamp().nsec()); + EXPECT_DOUBLE_EQ(0, msg.diff_pressure()); + EXPECT_DOUBLE_EQ(288.14999389648438, msg.temperature()); +} diff --git a/test/integration/apply_link_wrench_system.cc b/test/integration/apply_link_wrench_system.cc index 1578cac3d3..217c76941e 100644 --- a/test/integration/apply_link_wrench_system.cc +++ b/test/integration/apply_link_wrench_system.cc @@ -139,6 +139,9 @@ TEST_F(ApplyLinkWrenchTestFixture, link4 = Link(model4.CanonicalLink(_ecm)); EXPECT_TRUE(link4.Valid(_ecm)); link4.EnableAccelerationChecks(_ecm); + link4 = Link(model4.CanonicalLink(_ecm)); + EXPECT_TRUE(link4.Valid(_ecm)); + link4.EnableAccelerationChecks(_ecm); }) .OnPostUpdate([&]( const UpdateInfo &, diff --git a/test/integration/battery_plugin.cc b/test/integration/battery_plugin.cc index 2d54f47e1d..6873341208 100644 --- a/test/integration/battery_plugin.cc +++ b/test/integration/battery_plugin.cc @@ -33,12 +33,14 @@ #include "gz/sim/Server.hh" #include "gz/sim/ServerConfig.hh" #include "gz/sim/SystemLoader.hh" -#include "test_config.hh" #include "gz/sim/Entity.hh" #include "gz/sim/components/BatterySoC.hh" +#include "gz/sim/components/BatteryPowerLoad.hh" #include "gz/sim/components/Link.hh" #include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "test_config.hh" #include "plugins/MockSystem.hh" #include "../helpers/EnvTestFixture.hh" @@ -106,7 +108,7 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleBattery)) components::BatterySoC::typeId)); auto batComp = ecm->Component(batEntity); - // Check voltage is never zero. + // Check state of charge is never zero. // This check is here to guarantee that components::BatterySoC in // the LinearBatteryPlugin is not zero when created. If // components::BatterySoC is zero on start, then the Physics plugin @@ -134,8 +136,8 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleBattery)) components::BatterySoC::typeId)); auto batComp = ecm->Component(batEntity); - // Check voltage after consumption is lower than initial voltage - EXPECT_LT(batComp->Data(), 12.592); + // Check state of charge after consumption is lower than 1 (full charge). + EXPECT_LT(batComp->Data(), 1); // Check there is a single battery matching exactly the one specified int linearBatCount = 0; @@ -152,8 +154,8 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleBattery)) EXPECT_NE(kNullEntity, _batEntity); EXPECT_EQ(_nameComp->Data(), "linear_battery"); - // Check battery component voltage data is lower than initial voltage - EXPECT_LT(_batComp->Data(), 12.592); + // Check state of charge is lower than initial charge. + EXPECT_LT(_batComp->Data(), 1); } return true; @@ -163,8 +165,175 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleBattery)) } ///////////////////////////////////////////////// -// Battery with power draining topics -// See https://github.com/gazebosim/gz-sim/issues/1175 +// Single battery with 1 extra consumer +TEST_F(BatteryPluginTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(SingleBatteryMultipleConsumers)) +{ + const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "battery.sdf"); + sdf::Root root; + EXPECT_EQ(root.Load(sdfPath).size(), 0lu); + EXPECT_GT(root.WorldCount(), 0lu); + + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfPath); + + // A pointer to the ecm. This will be valid once we run the mock system + EntityComponentManager *ecm = nullptr; + this->mockSystem->preUpdateCallback = + [&ecm](const UpdateInfo &, EntityComponentManager &_ecm) + { + ecm = &_ecm; + + // Check a battery exists + EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId)); + + // Find the battery entity + Entity batEntity = ecm->EntityByComponents(components::Name( + "linear_battery")); + EXPECT_NE(kNullEntity, batEntity); + + // Find the battery component + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity, + components::BatterySoC::typeId)); + auto batComp = ecm->Component(batEntity); + + // Check state of charge is never zero. + // This check is here to guarantee that components::BatterySoC in + // the LinearBatteryPlugin is not zero when created. If + // components::BatterySoC is zero on start, then the Physics plugin + // can disable a joint. This in turn can prevent the joint from + // rotating. See https://github.com/gazebosim/gz-sim/issues/55 + EXPECT_GT(batComp->Data(), 0); + }; + + // Start server + Server server(serverConfig); + server.AddSystem(this->systemPtr); + server.Run(true, 100, false); + EXPECT_NE(nullptr, ecm); + + // Check a battery exists + EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId)); + + // Find the battery entity + Entity batEntity = ecm->EntityByComponents(components::Name( + "linear_battery")); + EXPECT_NE(kNullEntity, batEntity); + + // Find the battery component. + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity, + components::BatterySoC::typeId)); + auto batComp = ecm->Component(batEntity); + + // Check state of charge after consumption is lower than initial one + EXPECT_LT(batComp->Data(), 1); + + auto batLoad = batComp->Data(); + + // Add Entity with a battery power load component + Entity consumerEntity = ecm->CreateEntity(); + components::BatteryPowerLoadInfo batteryPowerLoadInfo{batEntity, 500}; + ecm->CreateComponent(consumerEntity, + components::BatteryPowerLoad(batteryPowerLoadInfo)); + + // Reset battery state of charge and run the server + batComp->Data() = 1; + EXPECT_DOUBLE_EQ(batComp->Data(), 1.0); + server.Run(true, 100, false); + + // Battery consumed this time should be lower than before due to + // the extra consumer + EXPECT_LT(batComp->Data(), batLoad); +} + +///////////////////////////////////////////////// +// Two models with its own battery, one with one extra consumer. +TEST_F(BatteryPluginTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(BatteriesDifferentConsumers)) +{ + const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "battery_thruster_consumer.sdf"); + sdf::Root root; + EXPECT_EQ(root.Load(sdfPath).size(), 0lu); + EXPECT_GT(root.WorldCount(), 0lu); + + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfPath); + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + this->mockSystem->preUpdateCallback = + [&ecm](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + { + ecm = &_ecm; + + // Check a battery exists + EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId)); + + // Find the battery entities + Entity batEntity = ecm->EntityByComponents(components::Name( + "linear_battery")); + EXPECT_NE(kNullEntity, batEntity); + Entity batEntity2 = ecm->EntityByComponents(components::Name( + "linear_battery2")); + EXPECT_NE(kNullEntity, batEntity2); + + // Find the battery components + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity, + components::BatterySoC::typeId)); + auto batComp = ecm->Component(batEntity); + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity2, + components::BatterySoC::typeId)); + auto batComp2 = ecm->Component(batEntity2); + + // Check state of charge is never zero. + // This check is here to guarantee that components::BatterySoC in + // the LinearBatteryPlugin is not zero when created. If + // components::BatterySoC is zero on start, then the Physics plugin + // can disable a joint. This in turn can prevent the joint from + // rotating. See https://github.com/ignitionrobotics/ign-gazebo/issues/55 + EXPECT_GT(batComp->Data(), 0); + EXPECT_GT(batComp2->Data(), 0); + }; + + // Start server + Server server(serverConfig); + server.AddSystem(this->systemPtr); + server.Run(true, 100, false); + EXPECT_NE(nullptr, ecm); + + // Check a battery exists + EXPECT_TRUE(ecm->HasComponentType(components::BatterySoC::typeId)); + + // Find the battery entities + Entity batEntity = ecm->EntityByComponents(components::Name( + "linear_battery")); + EXPECT_NE(kNullEntity, batEntity); + Entity batEntity2 = ecm->EntityByComponents(components::Name( + "linear_battery2")); + EXPECT_NE(kNullEntity, batEntity2); + + // Find the batteries components. + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity, + components::BatterySoC::typeId)); + auto batComp = ecm->Component(batEntity); + EXPECT_TRUE(ecm->EntityHasComponentType(batEntity2, + components::BatterySoC::typeId)); + auto batComp2 = ecm->Component(batEntity2); + + // Check state of charge after consumption is lower than initial one + EXPECT_LT(batComp->Data(), 1); + EXPECT_LT(batComp2->Data(), 1); + + // Check state of charge of the battery with an extra consumer is lower + // than the one without it. + EXPECT_LT(batComp2->Data(), batComp->Data()); +} + +///////////////////////////////////////////////// +// Battery with power draining topics +// See https://github.com/ignitionrobotics/ign-gazebo/issues/1175 TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PowerDrainTopic)) { const auto sdfPath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), @@ -196,7 +365,7 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PowerDrainTopic)) components::BatterySoC::typeId)); auto batComp = ecm->Component(batEntity); - // Check voltage is never zero. + // Check state of charge is never zero. // This check is here to guarantee that components::BatterySoC in // the LinearBatteryPlugin is not zero when created. If // components::BatterySoC is zero on start, then the Physics plugin @@ -225,13 +394,13 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PowerDrainTopic)) auto batComp = ecm->Component(batEntity); // Check state of charge should be 1, since the batery has not drained - // and the is equivalent ot the . + // and the is equivalent to the . EXPECT_DOUBLE_EQ(batComp->Data(), 1.0); // Send a message on one of the topics, which will // start the battery draining when the server starts again. gz::transport::Node node; - auto pub = node.Advertise("/battery/discharge2"); + auto pub = node.Advertise("/battery/discharge"); msgs::StringMsg msg; pub.Publish(msg); @@ -240,5 +409,17 @@ TEST_F(BatteryPluginTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(PowerDrainTopic)) // The state of charge should be <1, since the batery has started // draining. + double stateOfCharge = batComp->Data(); EXPECT_LT(batComp->Data(), 1.0); + + // Send a message on one of the topics, which + // will stop the battery draining when the server starts again. + auto pub2 = node.Advertise("/battery/stop_discharge"); + pub2.Publish(msg); + + // Run the server again. + server.Run(true, 100, false); + + // The state of charge should be the same since the discharge was stopped + EXPECT_DOUBLE_EQ(batComp->Data(), stateOfCharge); } diff --git a/test/integration/components.cc b/test/integration/components.cc index 475993b092..37e75f4bbf 100644 --- a/test/integration/components.cc +++ b/test/integration/components.cc @@ -77,6 +77,7 @@ #include "gz/sim/components/PerformerLevels.hh" #include "gz/sim/components/PhysicsEnginePlugin.hh" #include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Projector.hh" #include "gz/sim/components/Scene.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/SourceFilePath.hh" @@ -1783,6 +1784,35 @@ TEST_F(ComponentsTest, ParticleEmitterCmd) EXPECT_EQ(comp1.Data().name(), comp3.Data().name()); } +////////////////////////////////////////////////// +TEST_F(ComponentsTest, Projector) +{ + // Create components + sdf::Projector projector1; + projector1.SetName("projector1"); + projector1.SetRawPose(math::Pose3d(0, 3, 4, GZ_PI, 0, 0)); + projector1.SetNearClip(1.5); + projector1.SetFarClip(10.3); + projector1.SetHorizontalFov(math::Angle(3.0)); + projector1.SetVisibilityFlags(0xFE); + projector1.SetTexture("path_to_texture"); + auto comp1 = components::Projector(projector1); + + // stream operators + std::ostringstream ostr; + comp1.Serialize(ostr); + std::istringstream istr(ostr.str()); + components::Projector comp3; + comp3.Deserialize(istr); + EXPECT_EQ("projector1", comp3.Data().Name()); + EXPECT_EQ(math::Pose3d(0, 3, 4, GZ_PI, 0, 0), comp3.Data().RawPose()); + EXPECT_DOUBLE_EQ(1.5, comp3.Data().NearClip()); + EXPECT_DOUBLE_EQ(10.3, comp3.Data().FarClip()); + EXPECT_EQ(math::Angle(3.0), comp3.Data().HorizontalFov()); + EXPECT_EQ(0xFE, comp3.Data().VisibilityFlags()); + EXPECT_EQ("path_to_texture", comp3.Data().Texture()); +} + ////////////////////////////////////////////////// TEST_F(ComponentsTest, JointTransmittedWrench) { diff --git a/test/integration/environmental_sensor_system.cc b/test/integration/environmental_sensor_system.cc index 71e4fda198..429af8f31b 100644 --- a/test/integration/environmental_sensor_system.cc +++ b/test/integration/environmental_sensor_system.cc @@ -24,6 +24,8 @@ #include "gz/sim/TestFixture.hh" #include +#include +#include #include "gz/transport/Node.hh" #include "test_config.hh" @@ -36,32 +38,61 @@ using namespace sim; /// \brief Test EnvironmentPreload system class EnvironmentSensorTest : public InternalFixture<::testing::Test> { - ///////////////////////////////////////////////// - public: EnvironmentSensorTest() - { - node.Subscribe("/world/environmental_sensor_test/" - "model/model_with_sensor/link/link/sensor/custom_sensor/" - "environmental_sensor/humidity", - &EnvironmentSensorTest::OnReceiveMsg, - this); - } - - ///////////////////////////////////////////////// - public: void OnReceiveMsg(const gz::msgs::Double& msg) - { - // Data set is such that sensor value == time for the first second. - double nsec = msg.header().stamp().nsec(); - double sec = static_cast(msg.header().stamp().sec()); - auto time = nsec * 1e-9 + sec; - EXPECT_NEAR(time, msg.data(), 1e-9); - this->receivedData = true; - } - - ///////////////////////////////////////////////// - public: std::atomic receivedData{false}; - - ///////////////////////////////////////////////// - public: transport::Node node; + ///////////////////////////////////////////////// + public: EnvironmentSensorTest() + { + node.Subscribe("/world/environmental_sensor_test/" + "model/model_with_sensor/link/link/sensor/custom_sensor/" + "environmental_sensor/humidity", + &EnvironmentSensorTest::OnReceiveMsg, + this); + + node.Subscribe("/wind_speed2d", &EnvironmentSensorTest::OnWind2d, this); + node.Subscribe("/wind_speed3d", &EnvironmentSensorTest::OnWind3d, this); + } + + ///////////////////////////////////////////////// + public: void OnReceiveMsg(const gz::msgs::Double& _msg) + { + // Data set is such that sensor value == time for the first second. + double nsec = _msg.header().stamp().nsec(); + double sec = static_cast(_msg.header().stamp().sec()); + auto time = nsec * 1e-9 + sec; + EXPECT_NEAR(time, _msg.data(), 1e-9); + this->receivedData = true; + } + + ///////////////////////////////////////////////// + public: void OnWind2d(const gz::msgs::Vector3d& _msg) + { + EXPECT_NEAR(_msg.x(), 1, 1e-6); + EXPECT_NEAR(_msg.y(), 0, 1e-6); + EXPECT_NEAR(_msg.z(), 0, 1e-6); + this->receivedWind2dData = true; + } + + ///////////////////////////////////////////////// + public: void OnWind3d(const gz::msgs::Vector3d& _msg) + { + // Robot is rotated 90 degrees in yaw and has transform type set to local. + // Wind is in global +x and therefore in robot's local frame it will be -y + EXPECT_NEAR(_msg.x(), 0, 1e-3); + EXPECT_NEAR(_msg.y(), -1, 1e-3); + EXPECT_NEAR(_msg.z(), 0, 1e-3); + this->receivedWind3dData = true; + } + + ///////////////////////////////////////////////// + public: std::atomic receivedData{false}; + + ///////////////////////////////////////////////// + public: std::atomic receivedWind2dData{false}; + + ///////////////////////////////////////////////// + public: std::atomic receivedWind3dData{false}; + + ///////////////////////////////////////////////// + public: transport::Node node; }; @@ -81,4 +112,6 @@ TEST_F(EnvironmentSensorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(CanPreload)) // Run server server.Run(true, 1000, false); EXPECT_TRUE(this->receivedData.load()); + EXPECT_TRUE(this->receivedWind2dData.load()); + EXPECT_TRUE(this->receivedWind3dData.load()); } diff --git a/test/integration/hydrodynamics.cc b/test/integration/hydrodynamics.cc index bcfb53b072..0126c61389 100644 --- a/test/integration/hydrodynamics.cc +++ b/test/integration/hydrodynamics.cc @@ -50,6 +50,8 @@ class HydrodynamicsTest : public InternalFixture<::testing::Test> /// \param[in] _drag_coeff Body drag coefficient public: std::vector TestWorld(const std::string &_world, const std::string &_namespace); + + public: math::Vector3d defaultForce{0, 0, 10.0}; }; ////////////////////////////////////////////////// @@ -88,8 +90,7 @@ std::vector HydrodynamicsTest::TestWorld( body.EnableVelocityChecks(_ecm); // Add force - math::Vector3d force(0, 0, 10.0); - body.AddWorldForce(_ecm, force); + body.AddWorldForce(_ecm, this->defaultForce); }). OnPostUpdate([&](const UpdateInfo &/*_info*/, const EntityComponentManager &_ecm) @@ -114,7 +115,7 @@ std::vector HydrodynamicsTest::TestWorld( /// of the body when a force is applied. TEST_F(HydrodynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityTestinOil)) { - auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + auto world = common::joinPaths(std::string(PROJECT_BINARY_PATH), "test", "worlds", "hydrodynamics.sdf"); auto sphere1Vels = this->TestWorld(world, "sphere1"); @@ -153,9 +154,10 @@ TEST_F(HydrodynamicsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(VelocityTestinOil)) /// This test makes sure that the transforms of the hydrodynamics /// plugin are correct by comparing 3 cylinders in different /// positions and orientations. -TEST_F(HydrodynamicsTest, TransformsTestinWater) +TEST_F(HydrodynamicsTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(TransformsTestinWater)) { - auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + auto world = common::joinPaths(std::string(PROJECT_BINARY_PATH), "test", "worlds", "hydrodynamics.sdf"); auto cylinder1Vels = this->TestWorld(world, "cylinder1"); @@ -174,3 +176,28 @@ TEST_F(HydrodynamicsTest, TransformsTestinWater) EXPECT_NEAR(cylinder2Vels[i].Z(), cylinder3Vels[i].Z(), 1e-4); } } + +///////////////////////////////////////////////// +/// This tests the current. A current of (1, 0, 0) is loaded in via a csv file +TEST_F(HydrodynamicsTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(TransformsTestIn)) +{ + this->defaultForce = math::Vector3d(0, 0, 0); + auto world = common::joinPaths(std::string(PROJECT_BINARY_PATH), + "test", "worlds", "hydrodynamics.sdf"); + + auto sphereVel = this->TestWorld(world, "sphere_current"); + + for (unsigned int i = 990; i < 1000; ++i) + { + // Expect for the velocity to stabilize + EXPECT_NEAR(sphereVel[i-1].Z(), sphereVel[i].Z(), 1e-6); + EXPECT_NEAR(sphereVel[i-1].Y(), sphereVel[i].Y(), 1e-6); + EXPECT_NEAR(sphereVel[i-1].X(), sphereVel[i].X(), 1e-3); + + // Given current of (1,0,0), vehicle should move in similar direction. + EXPECT_NEAR(sphereVel[i-1].Z(), 0, 1e-6); + EXPECT_NEAR(sphereVel[i-1].Y(), 0, 1e-6); + EXPECT_GT(sphereVel[i-1].X(), 0); + } +} diff --git a/test/integration/hydrodynamics_flags.cc b/test/integration/hydrodynamics_flags.cc index b0b3a6694d..abcfd259ea 100644 --- a/test/integration/hydrodynamics_flags.cc +++ b/test/integration/hydrodynamics_flags.cc @@ -119,7 +119,8 @@ void HydrodynamicsFlagsTest::TestWorld(const std::string &_world, ///////////////////////////////////////////////// /// This test makes sure that the linear velocity is reuduced /// disbling the coriolis force and also when disabling the added mass. -TEST_F(HydrodynamicsFlagsTest, AddedMassCoriolisFlags) +TEST_F(HydrodynamicsFlagsTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(AddedMassCoriolisFlags)) { auto world = common::joinPaths(std::string(PROJECT_SOURCE_PATH), "test", "worlds", "hydrodynamics_flags.sdf"); diff --git a/test/integration/include_sim_hh.cc b/test/integration/include_sim_hh.cc new file mode 100644 index 0000000000..2c32b2be8c --- /dev/null +++ b/test/integration/include_sim_hh.cc @@ -0,0 +1,27 @@ +/* + * 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 + +///////////////////////////////////////////////// +// Simple test to make sure it compiles +TEST(Compilation, sim_hh) +{ + gz::sim::System system; +} diff --git a/test/integration/joint.cc b/test/integration/joint.cc new file mode 100644 index 0000000000..12044025c6 --- /dev/null +++ b/test/integration/joint.cc @@ -0,0 +1,601 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class JointIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Joint joint; + EXPECT_FALSE(joint.Valid(ecm)); + } + + // Missing joint component + { + auto id = ecm.CreateEntity(); + Joint joint(id); + EXPECT_FALSE(joint.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + EXPECT_TRUE(joint.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No name + EXPECT_EQ(std::nullopt, joint.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("joint_name")); + EXPECT_EQ("joint_name", joint.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, JointNames) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No parent or child joint names + EXPECT_EQ(std::nullopt, joint.ParentLinkName(ecm)); + EXPECT_EQ(std::nullopt, joint.ChildLinkName(ecm)); + + // Add parent joint name + ecm.CreateComponent(id, + components::ParentLinkName("parent_joint_name")); + EXPECT_EQ("parent_joint_name", joint.ParentLinkName(ecm)); + + // Add child joint name + ecm.CreateComponent(id, + components::ChildLinkName("child_joint_name")); + EXPECT_EQ("child_joint_name", joint.ChildLinkName(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No pose + EXPECT_EQ(std::nullopt, joint.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, joint.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Type) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No joint type + EXPECT_EQ(std::nullopt, joint.Type(ecm)); + + // Add type + sdf::JointType jointType = sdf::JointType::PRISMATIC; + ecm.CreateComponent(id, + components::JointType(jointType)); + EXPECT_EQ(jointType, joint.Type(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Axis) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No joint axis + EXPECT_EQ(std::nullopt, joint.Axis(ecm)); + + // Add axis + sdf::JointAxis jointAxis; + auto errors = jointAxis.SetXyz(math::Vector3d(0, 1, 1)); + EXPECT_TRUE(errors.empty()); + ecm.CreateComponent(id, + components::JointAxis(jointAxis)); + EXPECT_EQ(jointAxis.Xyz(), (*joint.Axis(ecm))[0].Xyz()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ThreadPitch) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Joint()); + + Joint joint(id); + + // No name + EXPECT_EQ(std::nullopt, joint.ThreadPitch(ecm)); + + // Add name + ecm.CreateComponent(id, + components::ThreadPitch(1.23)); + EXPECT_DOUBLE_EQ(1.23, *joint.ThreadPitch(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SensorByName) +{ + EntityComponentManager ecm; + + // Joint + auto eJoint = ecm.CreateEntity(); + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + EXPECT_EQ(0u, joint.SensorCount(ecm)); + + // Sensor + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + ecm.CreateComponent(eSensor, + components::ParentEntity(eJoint)); + ecm.CreateComponent(eSensor, + components::Name("sensor_name")); + + // Check joint + EXPECT_EQ(eSensor, joint.SensorByName(ecm, "sensor_name")); + EXPECT_EQ(1u, joint.SensorCount(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetVelocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector velCmd{1}; + joint.SetVelocity(ecm, velCmd); + + // velocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(velCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity cmd is updated + std::vector velCmd2{0}; + joint.SetVelocity(ecm, velCmd2); + EXPECT_EQ(velCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetForce) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointForceCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector forceCmd{10}; + joint.SetForce(ecm, forceCmd); + + // velocity cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(forceCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the force cmd is updated + std::vector forceCmd2{1}; + joint.SetForce(ecm, forceCmd2); + EXPECT_EQ(forceCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetVelocityLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector velLimitsCmd{math::Vector2d(0.1, 1.1)}; + joint.SetVelocityLimits(ecm, velLimitsCmd); + + // velocity limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(velLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity limits cmd is updated + std::vector velLimitsCmd2{math::Vector2d(-0.2, 2.4)}; + joint.SetVelocityLimits(ecm, velLimitsCmd2); + EXPECT_EQ(velLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetEffortLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointEffortLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector effortLimitsCmd{math::Vector2d(9, 9.9)}; + joint.SetEffortLimits(ecm, effortLimitsCmd); + + // effort limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(effortLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the effort limits cmd is updated + std::vector effortLimitsCmd2{math::Vector2d(5.2, 5.4)}; + joint.SetEffortLimits(ecm, effortLimitsCmd2); + EXPECT_EQ(effortLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, SetPositionLimits) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointPositionLimitsCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector positionLimitsCmd{math::Vector2d(-0.1, 0.1)}; + joint.SetPositionLimits(ecm, positionLimitsCmd); + + // position limits cmd should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(positionLimitsCmd, + ecm.Component(eJoint)->Data()); + + // Make sure the position limits cmd is updated + std::vector positionLimitsCmd2{math::Vector2d(-0.2, 0.4)}; + joint.SetPositionLimits(ecm, positionLimitsCmd2); + EXPECT_EQ(positionLimitsCmd2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ResetVelocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointVelocityReset should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector vel{1}; + joint.ResetVelocity(ecm, vel); + + // velocity reset should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(vel, + ecm.Component(eJoint)->Data()); + + // Make sure the velocity reset is updated + std::vector vel2{0}; + joint.ResetVelocity(ecm, vel2); + EXPECT_EQ(vel2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ResetPosition) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // No JointPositionReset should exist by default + EXPECT_EQ(nullptr, ecm.Component(eJoint)); + + std::vector pos{1}; + joint.ResetPosition(ecm, pos); + + // position reset should exist + EXPECT_NE(nullptr, ecm.Component(eJoint)); + EXPECT_EQ(pos, + ecm.Component(eJoint)->Data()); + + // Make sure the position reset is updated + std::vector pos2{0}; + joint.ResetPosition(ecm, pos2); + EXPECT_EQ(pos2, + ecm.Component(eJoint)->Data()); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Velocity) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, velocity should return nullopt + EXPECT_EQ(std::nullopt, joint.Velocity(ecm)); + + // After enabling, velocity should return default values + joint.EnableVelocityCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector velOut = *joint.Velocity(ecm); + EXPECT_TRUE(velOut.empty()); + + // With custom velocities + std::vector vel{0.3, 0.4}; + ecm.SetComponentData(eJoint, vel); + velOut = *joint.Velocity(ecm); + EXPECT_EQ(2u, velOut.size()); + EXPECT_DOUBLE_EQ(vel[0], velOut[0]); + EXPECT_DOUBLE_EQ(vel[1], velOut[1]); + + // Disabling velocities goes back to nullopt + joint.EnableVelocityCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.Velocity(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, Position) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, position should return nullopt + EXPECT_EQ(std::nullopt, joint.Position(ecm)); + + // After enabling, position should return default values + joint.EnablePositionCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector posOut = *joint.Position(ecm); + EXPECT_TRUE(posOut.empty()); + + // With custom positions + std::vector pos{-0.3, -0.4}; + ecm.SetComponentData(eJoint, pos); + posOut = *joint.Position(ecm); + EXPECT_EQ(2u, posOut.size()); + EXPECT_DOUBLE_EQ(pos[0], posOut[0]); + EXPECT_DOUBLE_EQ(pos[1], posOut[1]); + + // Disabling positions goes back to nullopt + joint.EnablePositionCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.Position(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, TransmittedWrench) +{ + EntityComponentManager ecm; + + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Before enabling, wrench should return nullopt + EXPECT_EQ(std::nullopt, joint.TransmittedWrench(ecm)); + + // After enabling, wrench should return default values + joint.EnableTransmittedWrenchCheck(ecm); + + EXPECT_NE(nullptr, ecm.Component(eJoint)); + + std::vector wrenchOut = *joint.TransmittedWrench(ecm); + // todo(anyone) Unlike Velocity and Position functions that return an + // empty vector if it has not been populated yet, the wrench vector + // will contain one empty wrench msg. This is because the TransmittedWrench + // API workarounds the fact that the TransmittedWrench component contains + // only one wrench reading instead of a wrench vector like positions and + // velocities. + // EXPECT_TRUE(wrenchOut.empty()); + + // With custom wrench + msgs::Wrench wrenchMsg; + msgs::Set(wrenchMsg.mutable_force(), + math::Vector3d(0.2, 3.2, 0.1)); + + std::vector wrench{wrenchMsg}; + ecm.SetComponentData(eJoint, wrench[0]); + wrenchOut = *joint.TransmittedWrench(ecm); + EXPECT_EQ(1u, wrenchOut.size()); + EXPECT_DOUBLE_EQ(wrench[0].force().x(), wrenchOut[0].force().x()); + EXPECT_DOUBLE_EQ(wrench[0].force().y(), wrenchOut[0].force().y()); + EXPECT_DOUBLE_EQ(wrench[0].force().z(), wrenchOut[0].force().z()); + + // Disabling wrench goes back to nullopt + joint.EnableTransmittedWrenchCheck(ecm, false); + EXPECT_EQ(std::nullopt, joint.TransmittedWrench(ecm)); + EXPECT_EQ(nullptr, ecm.Component(eJoint)); +} + +////////////////////////////////////////////////// +TEST_F(JointIntegrationTest, ParentModel) +{ + EntityComponentManager ecm; + + // Model + auto eModel = ecm.CreateEntity(); + ecm.CreateComponent(eModel, components::Model()); + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + + Joint joint(eJoint); + EXPECT_EQ(eJoint, joint.Entity()); + EXPECT_FALSE(joint.ParentModel(ecm).has_value()); + + ecm.CreateComponent(eJoint, + components::ParentEntity(eModel)); + + ASSERT_TRUE(joint.Valid(ecm)); + + // Check parent model + EXPECT_EQ(eModel, ecm.ParentEntity(eJoint)); + auto parentModel = joint.ParentModel(ecm); + ASSERT_TRUE(parentModel.has_value()); + EXPECT_TRUE(parentModel->Valid(ecm)); + EXPECT_EQ(eModel, parentModel->Entity()); +} diff --git a/test/integration/joint_controller_system.cc b/test/integration/joint_controller_system.cc index 2ed5addbcd..894a9e5196 100644 --- a/test/integration/joint_controller_system.cc +++ b/test/integration/joint_controller_system.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2019 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,6 +19,7 @@ #include #include +#include #include #include @@ -55,9 +57,9 @@ TEST_F(JointControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_controller.sdf"; - serverConfig.SetSdfFile(sdfFile); + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_controller.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -144,6 +146,119 @@ TEST_F(JointControllerTestFixture, } } +///////////////////////////////////////////////// +// Tests that the JointController accepts joint velocity commands +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(JointControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32( + JointVelocityMultipleJointsSubTopicCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_controller.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // Publish command and check that the joint velocity is set + transport::Node node; + auto pub = node.Advertise( + "/model/joint_controller_test/joints"); + + const double testAngVel{10.0}; + msgs::Double msg; + msg.set_data(testAngVel); + + pub.Publish(msg); +} + +///////////////////////////////////////////////// +// Tests that the JointController accepts actuator commands +TEST_F(JointControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32( + JointControllerActuatorsCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_controller.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string linkName = "rotor4"; + + test::Relay testSystem; + math::Vector3d angularVelocity; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto link = _ecm.EntityByComponents(components::Link(), + components::Name(linkName)); + // Create an AngularVelocity component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(link)) + { + _ecm.CreateComponent(link, components::AngularVelocity()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Link *, + const components::Name *_name, + const components::AngularVelocity *_angularVel) -> bool + { + EXPECT_EQ(_name->Data(), linkName); + angularVelocity = _angularVel->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, angularVelocity.Length(), TOL); + + // Publish command and check that the joint velocity is set + transport::Node node; + auto pub = node.Advertise( + "/actuators"); + + const double testAngVel{10.0}; + msgs::Actuators msg; + msg.add_velocity(testAngVel); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 3000; + server.Run(true, testIters , false); + + EXPECT_NEAR(0, angularVelocity.X(), 1e-2); + EXPECT_NEAR(0, angularVelocity.Y(), 1e-2); + EXPECT_NEAR(testAngVel, angularVelocity.Z(), 1e-2); +} + ///////////////////////////////////////////////// // Tests the JointController using joint force commands TEST_F(JointControllerTestFixture, @@ -153,9 +268,10 @@ TEST_F(JointControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_controller.sdf"; - serverConfig.SetSdfFile(sdfFile); + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_controller.sdf")); + Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -163,7 +279,7 @@ TEST_F(JointControllerTestFixture, server.SetUpdatePeriod(0ns); - const std::string linkName = "rotor2"; + const std::string linkName = "rotor3"; test::Relay testSystem; math::Vector3d angularVelocity; @@ -230,9 +346,9 @@ TEST_F(JointControllerTestFixture, InexistentJoint) // Start server ServerConfig serverConfig; - const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), - "test", "worlds", "joint_controller_invalid.sdf"); - serverConfig.SetSdfFile(sdfFile); + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_controller_invalid.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index e80b6d3e4f..0df56e165f 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -1,5 +1,6 @@ /* * Copyright (C) 2020 Open Source Robotics Foundation + * Copyright (C) 2023 Benjamin Perseghetti, Rudis Laboratories * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -18,6 +19,7 @@ #include #include +#include #include #include @@ -26,10 +28,13 @@ #include "gz/sim/components/Joint.hh" #include "gz/sim/components/JointPosition.hh" +#include "gz/sim/components/Model.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/Server.hh" #include "gz/sim/SystemLoader.hh" +#include "gz/sim/Util.hh" + #include "test_config.hh" #include "../helpers/Relay.hh" @@ -56,9 +61,9 @@ TEST_F(JointPositionControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_position_controller.sdf"; - serverConfig.SetSdfFile(sdfFile); + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -126,16 +131,17 @@ TEST_F(JointPositionControllerTestFixture, ///////////////////////////////////////////////// // Tests that the JointPositionController accepts joint position commands +// See https://github.com/gazebosim/gz-sim/issues/1175 TEST_F(JointPositionControllerTestFixture, - GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositonVelocityCommand)) + GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionActuatorsCommand)) { using namespace std::chrono_literals; // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_position_controller_velocity.sdf"; - serverConfig.SetSdfFile(sdfFile); + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller_actuators.sdf")); Server server(serverConfig); EXPECT_FALSE(server.Running()); @@ -148,7 +154,7 @@ TEST_F(JointPositionControllerTestFixture, test::Relay testSystem; std::vector currentPosition; testSystem.OnPreUpdate( - [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + [&](const UpdateInfo &, EntityComponentManager &_ecm) { auto joint = _ecm.EntityByComponents(components::Joint(), components::Name(jointName)); @@ -160,12 +166,89 @@ TEST_F(JointPositionControllerTestFixture, } }); - testSystem.OnPostUpdate([&](const sim::UpdateInfo &, - const sim::EntityComponentManager &_ecm) + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) { _ecm.Each( - [&](const gz::sim::Entity &, + [&](const Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + EXPECT_EQ(_name->Data(), jointName); + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/actuators"); + + const double targetPosition{1.0}; + msgs::Actuators msg; + msg.add_position(targetPosition); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 3000; + server.Run(true, testIters , false); + + EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); +} + +///////////////////////////////////////////////// +// Tests that the JointPositionController accepts joint position commands +TEST_F(JointPositionControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositionVelocityCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller_velocity.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j1"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, const components::Joint *, const components::Name *_name, const components::JointPosition *_position) -> bool @@ -207,3 +290,194 @@ TEST_F(JointPositionControllerTestFixture, EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); } + +///////////////////////////////////////////////// +// Tests that the JointPositionController accepts joint position +// sub_topic commands +TEST_F(JointPositionControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32( + JointPositionMultipleJointsSubTopicCommand)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller_multiple_joints_subtopic.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j1"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + EXPECT_EQ(_name->Data(), jointName); + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + const std::size_t initIters = 10; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model/joint_position_controller_test/joints"); + + const double targetPosition{2.0}; + msgs::Double msg; + msg.set_data(targetPosition); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 1000; + server.Run(true, testIters , false); + + EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); +} + +///////////////////////////////////////////////// +// Tests that the JointPositionController commands joints in +// nested models. +TEST_F(JointPositionControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32( + JointPositionNestedModels)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths( + PROJECT_SOURCE_PATH, "test", "worlds", + "joint_position_controller_nested_models.sdf")); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + // model2 contains two joints with the same name, one is nested. + const std::string modelName = "model2"; + const std::string jointName = "rotor_joint"; + const std::string scopedJointName = "model21::rotor_joint"; + + test::Relay testSystem; + std::vector joint2Position; + std::vector joint21Position; + testSystem.OnPreUpdate( + [&](const sim::UpdateInfo &, sim::EntityComponentManager &_ecm) + { + // Create a JointPosition component if it doesn't exist. + _ecm.Each( + [&](const gz::sim::Entity &_joint, + const components::Joint *, + const components::Name *_name) -> bool + { + if (_name->Data() == jointName) + { + _ecm.CreateComponent(_joint, components::JointPosition()); + } + return true; + }); + }); + + testSystem.OnPostUpdate([&](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + // Retrieve the parent model. + Entity model{kNullEntity}; + _ecm.Each( + [&](const gz::sim::Entity &_entity, + const components::Model *, + const components::Name *_name) -> bool + { + if (_name->Data() == modelName) + { + model = _entity; + } + return true; + }); + + // joint2 + { + auto entities = entitiesFromScopedName(jointName, _ecm, model); + Entity joint = *entities.begin(); + auto posComp = _ecm.Component(joint); + joint2Position = posComp->Data(); + } + + // joint21 + { + auto entities = entitiesFromScopedName(scopedJointName, _ecm, model); + Entity joint = *entities.begin(); + auto posComp = _ecm.Component(joint); + joint21Position = posComp->Data(); + } + }); + + server.AddSystem(testSystem.systemPtr); + + // joint pos starts at 0 + const std::size_t initIters = 1; + server.Run(true, initIters, false); + EXPECT_NEAR(0, joint2Position.at(0), TOL); + EXPECT_NEAR(0, joint21Position.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model21/cmd_rotor"); + + const double targetPosition{1.0}; + msgs::Double msg; + msg.set_data(targetPosition); + + pub.Publish(msg); + // Wait for the message to be published + std::this_thread::sleep_for(100ms); + + const std::size_t testIters = 1000; + server.Run(true, testIters , false); + + // joint2 should not move + EXPECT_NEAR(0, joint2Position.at(0), TOL); + + // joint21 should be at target position + EXPECT_NEAR(targetPosition, joint21Position.at(0), TOL); +} diff --git a/test/integration/light.cc b/test/integration/light.cc new file mode 100644 index 0000000000..292d0b8366 --- /dev/null +++ b/test/integration/light.cc @@ -0,0 +1,804 @@ +/* + * 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 + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class LightIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Light light; + EXPECT_FALSE(light.Valid(ecm)); + } + + // Missing light component + { + auto id = ecm.CreateEntity(); + Light light(id); + EXPECT_FALSE(light.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + EXPECT_TRUE(light.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + // No name + EXPECT_EQ(std::nullopt, light.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("light_name")); + EXPECT_EQ("light_name", light.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + // No pose + EXPECT_EQ(std::nullopt, light.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, light.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Type) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + // No light type + EXPECT_EQ(std::nullopt, light.Type(ecm)); + + // Add type + std::string lightType = "point"; + ecm.CreateComponent(id, + components::LightType(lightType)); + EXPECT_EQ(lightType, light.Type(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, CastShadows) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + lightSdf.SetCastShadows(true); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_TRUE(*light.CastShadows(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Intensity) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double intensity = 0.2; + lightSdf.SetIntensity(intensity); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(intensity, *light.Intensity(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Direction) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Vector3d dir(1.0, 0.0, 0.0); + lightSdf.SetDirection(dir); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(dir, *light.Direction(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, DiffuseColor) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Color color(1.0, 1.0, 0.0); + lightSdf.SetDiffuse(color); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(color, *light.DiffuseColor(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpecularColor) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Color color(1.0, 1.0, 0.0); + lightSdf.SetSpecular(color); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(color, *light.SpecularColor(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationRange) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double range = 2.4; + lightSdf.SetAttenuationRange(range); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(range, *light.AttenuationRange(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationConstant) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double value = 0.2; + lightSdf.SetConstantAttenuationFactor(value); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(value, *light.AttenuationConstant(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationLinear) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double value = 0.1; + lightSdf.SetLinearAttenuationFactor(value); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(value, *light.AttenuationLinear(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, AttenuationQuadratic) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double value = 0.01; + lightSdf.SetQuadraticAttenuationFactor(value); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(value, *light.AttenuationQuadratic(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpotInnerAngle) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Angle angle(1.57); + lightSdf.SetSpotInnerAngle(angle); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(angle, *light.SpotInnerAngle(ecm)); +} + + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpotOuterAngle) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + math::Angle angle(2.3); + lightSdf.SetSpotOuterAngle(angle); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_EQ(angle, *light.SpotOuterAngle(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SpotFalloff) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Light()); + + Light light(id); + + sdf::Light lightSdf; + double falloff(0.3); + lightSdf.SetSpotFalloff(falloff); + ecm.CreateComponent(id, + components::Light(lightSdf)); + EXPECT_DOUBLE_EQ(falloff, *light.SpotFalloff(ecm)); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetPose) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Pose3d poseCmd(0.1, -2, 30, 0.2, 0.3, 0.8); + light.SetPose(ecm, poseCmd); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(poseCmd, msgs::Convert( + ecm.Component(eLight)->Data().pose())); + + // Make sure the light cmd is updated + math::Pose3d poseCmd2(9.3, -8, -1, 0.0, -0.3, 3.8); + light.SetPose(ecm, poseCmd2); + EXPECT_EQ(poseCmd2, msgs::Convert( + ecm.Component(eLight)->Data().pose())); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetCastShadows) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + bool castShadows = true; + light.SetCastShadows(ecm, castShadows); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(castShadows, + ecm.Component(eLight)->Data().cast_shadows()); + + // Make sure the light cmd is updated + bool castShadows2 = false; + light.SetCastShadows(ecm, castShadows2); + EXPECT_EQ(castShadows2, + ecm.Component(eLight)->Data().cast_shadows()); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetIntensity) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double intensity = 0.3; + light.SetIntensity(ecm, intensity); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(intensity, + ecm.Component(eLight)->Data().intensity()); + + // Make sure the light cmd is updated + double intensity2 = 0.001; + light.SetIntensity(ecm, intensity2); + EXPECT_FLOAT_EQ(intensity2, + ecm.Component(eLight)->Data().intensity()); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetDirection) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Vector3d dir(0.3, 0.4, 0.6); + light.SetDirection(ecm, dir); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(dir, msgs::Convert( + ecm.Component(eLight)->Data().direction())); + + // Make sure the light cmd is updated + math::Vector3d dir2(1.0, 0.0, 0.0); + light.SetDirection(ecm, dir2); + EXPECT_EQ(dir2, msgs::Convert( + ecm.Component(eLight)->Data().direction())); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetDiffuseColor) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Color color(1.0, 0.0, 1.0); + light.SetDiffuseColor(ecm, color); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(color, msgs::Convert( + ecm.Component(eLight)->Data().diffuse())); + + // Make sure the light cmd is updated + math::Color color2(1.0, 0.5, 0.5); + light.SetDiffuseColor(ecm, color2); + EXPECT_EQ(color2, msgs::Convert( + ecm.Component(eLight)->Data().diffuse())); +} + +///////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpecularColor) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Color color(1.0, 1.0, 0.0); + light.SetSpecularColor(ecm, color); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_EQ(color, msgs::Convert( + ecm.Component(eLight)->Data().specular())); + + // Make sure the light cmd is updated + math::Color color2(0.0, 1.0, 0.0); + light.SetSpecularColor(ecm, color2); + EXPECT_EQ(color2, msgs::Convert( + ecm.Component(eLight)->Data().specular())); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationRange) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double range = 56.2; + light.SetAttenuationRange(ecm, range); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(range, + ecm.Component(eLight)->Data().range()); + + // Make sure the light cmd is updated + double range2 = 2777.9; + light.SetAttenuationRange(ecm, range2); + EXPECT_FLOAT_EQ(range2, + ecm.Component(eLight)->Data().range()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationConstant) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double value = 3.0; + light.SetAttenuationConstant(ecm, value); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(value, + ecm.Component(eLight)->Data().attenuation_constant()); + + // Make sure the light cmd is updated + double value2 = 5.0; + light.SetAttenuationConstant(ecm, value2); + EXPECT_FLOAT_EQ(value2, + ecm.Component(eLight)->Data().attenuation_constant()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationLinear) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double value = 0.1; + light.SetAttenuationLinear(ecm, value); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(value, + ecm.Component(eLight)->Data().attenuation_linear()); + + // Make sure the light cmd is updated + double value2 = 0.2; + light.SetAttenuationLinear(ecm, value2); + EXPECT_FLOAT_EQ(value2, + ecm.Component(eLight)->Data().attenuation_linear()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetAttenuationQuadratic) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double value = 0.3; + light.SetAttenuationQuadratic(ecm, value); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(value, + ecm.Component( + eLight)->Data().attenuation_quadratic()); + + // Make sure the light cmd is updated + double value2 = 0.7; + light.SetAttenuationQuadratic(ecm, value2); + EXPECT_FLOAT_EQ(value2, + ecm.Component( + eLight)->Data().attenuation_quadratic()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpotInnerAngle) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Angle angle(2.9); + light.SetSpotInnerAngle(ecm, angle.Radian()); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(angle.Radian(), + ecm.Component(eLight)->Data().spot_inner_angle()); + + // Make sure the light cmd is updated + math::Angle angle2(0.9); + light.SetSpotInnerAngle(ecm, angle2.Radian()); + EXPECT_FLOAT_EQ(angle2.Radian(), + ecm.Component(eLight)->Data().spot_inner_angle()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpotOuterAngle) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + math::Angle angle(3.1); + light.SetSpotOuterAngle(ecm, angle.Radian()); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(angle.Radian(), + ecm.Component(eLight)->Data().spot_outer_angle()); + + // Make sure the light cmd is updated + math::Angle angle2(0.8); + light.SetSpotOuterAngle(ecm, angle2.Radian()); + EXPECT_FLOAT_EQ(angle2.Radian(), + ecm.Component(eLight)->Data().spot_outer_angle()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, SetSpotFalloff) +{ + EntityComponentManager ecm; + + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + + ASSERT_TRUE(light.Valid(ecm)); + + // No LightCmd should exist by default + EXPECT_EQ(nullptr, ecm.Component(eLight)); + + double falloff = 0.3; + light.SetSpotFalloff(ecm, falloff); + + // light cmd should exist + EXPECT_NE(nullptr, ecm.Component(eLight)); + EXPECT_FLOAT_EQ(falloff, + ecm.Component(eLight)->Data().spot_falloff()); + + // Make sure the light cmd is updated + double falloff2 = 1.0; + light.SetSpotFalloff(ecm, falloff2); + EXPECT_FLOAT_EQ(falloff2, + ecm.Component(eLight)->Data().spot_falloff()); +} + +////////////////////////////////////////////////// +TEST_F(LightIntegrationTest, Parent) +{ + EntityComponentManager ecm; + + { + // Link as parent + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + EXPECT_FALSE(light.Parent(ecm).has_value()); + + ecm.CreateComponent(eLight, + components::ParentEntity(eLink)); + ASSERT_TRUE(light.Valid(ecm)); + + // Check parent link + EXPECT_EQ(eLink, ecm.ParentEntity(eLight)); + auto parentLink = light.Parent(ecm); + EXPECT_EQ(eLink, parentLink); + } + + { + // World as parent + auto eWorld = ecm.CreateEntity(); + ecm.CreateComponent(eWorld, components::World()); + auto eLight = ecm.CreateEntity(); + ecm.CreateComponent(eLight, components::Light()); + + Light light(eLight); + EXPECT_EQ(eLight, light.Entity()); + EXPECT_FALSE(light.Parent(ecm).has_value()); + + ecm.CreateComponent(eLight, + components::ParentEntity(eWorld)); + ASSERT_TRUE(light.Valid(ecm)); + + // Check parent world + EXPECT_EQ(eWorld, ecm.ParentEntity(eLight)); + auto parentWorld = light.Parent(ecm); + EXPECT_EQ(eWorld, parentWorld); + } +} diff --git a/test/integration/odometry_publisher.cc b/test/integration/odometry_publisher.cc index dace4d208c..19edce7753 100644 --- a/test/integration/odometry_publisher.cc +++ b/test/integration/odometry_publisher.cc @@ -495,11 +495,14 @@ class OdometryPublisherTest std::vector odomLinVels; std::vector odomAngVels; + std::vector odomAngs; google::protobuf::RepeatedField odomTwistCovariance; // Create function to store data from odometry messages std::function odomCb = [&](const msgs::OdometryWithCovariance &_msg) { + odomAngs.push_back(msgs::Convert(_msg.pose_with_covariance(). + pose().orientation())); odomLinVels.push_back(msgs::Convert(_msg.twist_with_covariance(). twist().linear())); odomAngVels.push_back(msgs::Convert(_msg.twist_with_covariance(). @@ -521,6 +524,7 @@ class OdometryPublisherTest // Verify the Gaussian noise. ASSERT_FALSE(odomLinVels.empty()); + ASSERT_FALSE(odomAngs.empty()); ASSERT_FALSE(odomAngVels.empty()); int n = odomLinVels.size(); diff --git a/test/integration/optical_tactile_plugin.cc b/test/integration/optical_tactile_plugin.cc index 7f98aa1760..6c93c6b50a 100644 --- a/test/integration/optical_tactile_plugin.cc +++ b/test/integration/optical_tactile_plugin.cc @@ -68,10 +68,10 @@ class OpticalTactilePluginTest : public InternalFixture<::testing::Test> _j * this->normalForces.step() + _i * 3 * sizeof(float); measuredPoint.X() = *reinterpret_cast( - &msgBuffer[msgBufferIndex]); + &msgBuffer[msgBufferIndex]); measuredPoint.Y() = *reinterpret_cast( - &msgBuffer[msgBufferIndex + sizeof(float)]); + &msgBuffer[msgBufferIndex + sizeof(float)]); measuredPoint.Z() = *reinterpret_cast( &msgBuffer[msgBufferIndex + 2*sizeof(float)]); diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index bee99946f1..027005a406 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -38,11 +38,16 @@ #include #include "gz/sim/Entity.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Link.hh" #include "gz/sim/Server.hh" #include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" #include "gz/sim/Util.hh" #include "test_config.hh" // NOLINT(build/include) +#include "gz/sim/components/AngularAcceleration.hh" +#include "gz/sim/components/AngularVelocity.hh" #include "gz/sim/components/AxisAlignedBox.hh" #include "gz/sim/components/CanonicalLink.hh" #include "gz/sim/components/Collision.hh" @@ -60,6 +65,7 @@ #include "gz/sim/components/JointVelocityLimitsCmd.hh" #include "gz/sim/components/JointVelocityReset.hh" #include "gz/sim/components/Link.hh" +#include "gz/sim/components/LinearAcceleration.hh" #include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/components/Material.hh" #include "gz/sim/components/Model.hh" @@ -115,7 +121,6 @@ TEST_F(PhysicsSystemFixture, CreatePhysicsWorld) server.Run(true, 1, false); EXPECT_FALSE(server.Running()); } - // TODO(addisu) add useful EXPECT calls } //////////////////////////////////////////////// @@ -2264,3 +2269,355 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, server->AddSystem(testSystem.systemPtr); server->Run(true, nIters, false); } + + +////////////////////////////////////////////////// +/// This test makes sure that non-link entities' components +/// are updated by the physics system if they have been enabled. +/// A collision is a non-link entity +TEST_F(PhysicsSystemFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(NonLinkComponentsAreUpdated)) +{ + ServerConfig serverConfig; + + const auto sdfFile = + std::string(PROJECT_SOURCE_PATH) + "/test/worlds/non_link_components.sdf"; + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + auto gravity = world->Gravity(); + ASSERT_TRUE(nullptr != world); + + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + + server.SetUpdatePeriod(1ms); + + const std::string collisionName{"collision"}; + const std::string collisionWithOffsetName{"collision_with_offset"}; + + // Create a system that records the pose, vel, acc of the collisions. + test::Relay testSystem; + + size_t iterations = 0u; + std::size_t nIters{100}; + std::size_t halfIters{nIters / 2}; + + std::vector collisionPoses; + std::vector linVels, angVels, worldLinVels, worldAngVels; + std::vector linAccs, angAccs, worldLinAccs, worldAngAccs; + testSystem.OnPreUpdate( + [&iterations, &nIters, &halfIters, + &collisionWithOffsetName](const UpdateInfo &, + EntityComponentManager &_ecm) + { + _ecm.EachNew( + [&](const Entity &_entity, const components::Collision *, + const components::Name *_name, + const components::ParentEntity *) -> bool + { + // we only enable the velocity, acceleration components for + // the collision_with_offset + if (_name->Data() == collisionWithOffsetName) + { + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, + _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + enableComponent(_ecm, + _entity); + enableComponent(_ecm, + _entity); + } + return true; + }); + + _ecm.Each( + [&](const Entity &, const components::Collision *, + const components::Name *_name, + const components::ParentEntity *_parentEntity) -> bool + { + // after half of the simulation, apply a varying wrench, on the + // link + if (_name->Data() == collisionWithOffsetName && + iterations >= halfIters) + { + gz::sim::Link parentLink(_parentEntity->Data()); + parentLink.EnableVelocityChecks(_ecm); + parentLink.EnableAccelerationChecks(_ecm); + using namespace gz::math; + // increasing force downward + auto force = + -Vector3d::UnitZ * (static_cast(iterations) / + static_cast(nIters)); + // increasing torque around z + auto torque = + Vector3d::UnitZ * (static_cast(iterations) / + static_cast(nIters)); + parentLink.AddWorldWrench(_ecm, force, torque); + } + return true; + }); + }); + + // save all the components history + testSystem.OnPostUpdate( + [&](const UpdateInfo &, const EntityComponentManager &_ecm) + { + _ecm.Each< + components::Collision, components::Name, components::WorldPose, + components::LinearVelocity, components::AngularVelocity, + components::WorldLinearVelocity, components::WorldAngularVelocity, + components::LinearAcceleration, components::AngularAcceleration, + components::WorldLinearAcceleration, + components::WorldAngularAcceleration>( + [&](const Entity &, const components::Collision *, + const components::Name *_name, + const components::WorldPose *_pose, + const components::LinearVelocity *_linearVel, + const components::AngularVelocity *_angularVel, + const components::WorldLinearVelocity *_worldLinearVel, + const components::WorldAngularVelocity *_worldAngularVel, + const components::LinearAcceleration *_linearAcc, + const components::AngularAcceleration *_angularAcc, + const components::WorldLinearAcceleration *_worldLinearAcc, + const components::WorldAngularAcceleration *_worldAngularAcc) + -> bool + { + EXPECT_TRUE(_name->Data() == collisionWithOffsetName); + if (_name->Data() == collisionWithOffsetName) + { + collisionPoses.push_back(_pose->Data()); + linVels.push_back(_linearVel->Data()); + angVels.push_back(_angularVel->Data()); + worldLinVels.push_back(_worldLinearVel->Data()); + worldAngVels.push_back(_worldAngularVel->Data()); + linAccs.push_back(_linearAcc->Data()); + angAccs.push_back(_angularAcc->Data()); + worldLinAccs.push_back(_worldLinearAcc->Data()); + worldAngAccs.push_back(_worldAngularAcc->Data()); + } + return true; + }); + ++iterations; + return true; + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, nIters, false); + + ASSERT_EQ(nIters, collisionPoses.size()); + ASSERT_EQ(nIters, linVels.size()); + ASSERT_EQ(nIters, angVels.size()); + ASSERT_EQ(nIters, worldLinVels.size()); + ASSERT_EQ(nIters, worldAngVels.size()); + ASSERT_EQ(nIters, linAccs.size()); + ASSERT_EQ(nIters, angAccs.size()); + ASSERT_EQ(nIters, worldLinAccs.size()); + ASSERT_EQ(nIters, worldAngAccs.size()); + ASSERT_EQ(nIters, iterations); + + // The collision offset should be taken into account in the world pose + EXPECT_NEAR(0.1, collisionPoses.front().Pos().X(), 1e-2); + EXPECT_NEAR(0.2, collisionPoses.front().Pos().Y(), 1e-2); + EXPECT_NEAR(2, collisionPoses.front().Pos().Z(), 1e-2); + + // Make sure the position is updated every time step + // (here, the model is falling down, so only z should be updated) + for (size_t i = 1; i < nIters; i++) + { + EXPECT_GT(collisionPoses[i - 1].Pos().Z(), collisionPoses[i].Pos().Z()) + << "Model should be falling down."; + } + + double normLinVelYz, normWorldLinVelXy; + double normLinAccYz, normWorldLinAccXy; + // the first part checks that: + // - the collision poses are updated correctly + // - linear velocities (world/local) are updated. + for (size_t i = 1; i < halfIters; i++) + { + // --- LIN ACC (WORLD, LOCAL) + // linear acc is constant = gravity (both world and local) + EXPECT_NEAR(linAccs[i].Length(), gravity.Length(), 1e-2); + EXPECT_NEAR(worldLinAccs[i].Length(), gravity.Length(), 1e-2); + // local should mostly be X , world should be -Z + EXPECT_NEAR(linAccs[i].X(), gravity.Length(), 1e-2) + << "Local linear acceleration should be along X axis"; + EXPECT_NEAR(worldLinAccs[i].Z(), -gravity.Length(), 1e-2) + << "World Linear acceleration should be along -Z axis"; + // on the local(world) YZ(XY) plane the acceleration should be 0 + normLinAccYz = + sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); + EXPECT_NEAR(normLinAccYz, 0, 1e-2) + << "Local Linear acceleration on YZ-plane should be 0"; + normWorldLinAccXy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + + worldLinAccs[i].Y() * worldLinAccs[i].Y()); + EXPECT_NEAR(normWorldLinAccXy, 0, 1e-2) + << "World Linear acceleration on XY-plane should be 0"; + + // --- LIN VEL (WORLD, LOCAL) + // linear velocity should keep increasing, both local and world + EXPECT_LT(linVels[i - 1].Length(), linVels[i].Length()) + << "Local linear velocity should keep increasing"; + EXPECT_LT(worldLinVels[i - 1].Length(), worldLinVels[i].Length()) + << "World linear velocity should keep increasing"; + // local should mostly be X , world should be -Z + EXPECT_GT(linVels[i - 1].X(), 0) + << "Local linear vel should be positive in x"; + EXPECT_LT(worldLinVels[i - 1].Z(), 0) + << "World linear vel should be negative in z"; + // on the local(world) YZ(XY) plane the velocity should be 0 + normLinVelYz = + sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); + EXPECT_NEAR(normLinVelYz, 0, 1e-2) + << "Local Linear velocity on YZ-plane should be zero"; + normWorldLinVelXy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + + worldLinVels[i].Y() * worldLinVels[i].Y()); + EXPECT_NEAR(normWorldLinVelXy, 0, 1e-2) + << "World Linear acceleration on XY-plane should be zero"; + + // --- ANG ACC (WORLD, LOCAL) + // angular acc is constant = 0 (both world and local) + EXPECT_NEAR(angAccs[i].Length(), 0, 1e-2); + EXPECT_NEAR(worldAngAccs[i].Length(), 0, 1e-2); + // --- ANG VEL (WORLD, LOCAL) + // angular velocity is constant + EXPECT_NEAR(angVels[i].Length(), 0, 1e-2); + EXPECT_NEAR(worldAngVels[i].Length(), 0, 1e-2); + } + + // Second part, we applied a varying wrench, and we should see: + // - linear acceleration (world/local) are updated + // - angular acceleration (world/local) are updated + // - angular velocities (world/local) are updated + for (size_t i = halfIters + 1; i < nIters; i++) + { + // --- LIN ACC (WORLD, LOCAL) + EXPECT_LT(linAccs[i - 1].Length(), linAccs[i].Length()) + << "Local Linear Acceleration should be increasing."; + EXPECT_LT(worldLinAccs[i - 1].Length(), worldLinAccs[i].Length()) + << "World Linear Acceleration should be increasing."; + // on the local(world) YZ(XY) plane the acceleration norm should be positive + normLinAccYz = + sqrt(linAccs[i].Y() * linAccs[i].Y() + linAccs[i].Z() * linAccs[i].Z()); + EXPECT_GT(normLinAccYz, 0) + << "Local Linear acceleration on YZ-plane should be positive"; + normWorldLinAccXy = sqrt(worldLinAccs[i].X() * worldLinAccs[i].X() + + worldLinAccs[i].Y() * worldLinAccs[i].Y()); + EXPECT_GT(normWorldLinAccXy, 0) + << "World Linear acceleration on XY-plane should be positive"; + // on the local(world) YZ(XY) plane the velocity should be positive + normLinVelYz = + sqrt(linVels[i].Y() * linVels[i].Y() + linVels[i].Z() * linVels[i].Z()); + EXPECT_GT(normLinVelYz, 0) + << "Local Linear velocity on YZ-plane should be positive"; + normWorldLinVelXy = sqrt(worldLinVels[i].X() * worldLinVels[i].X() + + worldLinVels[i].Y() * worldLinVels[i].Y()); + EXPECT_GT(normWorldLinVelXy, 0) + << "World Linear acceleration on XY-plane should be positive"; + + // --- ANG ACC (WORLD, LOCAL) + EXPECT_LT(angAccs[i - 1].Length(), angAccs[i].Length()) + << "Local Angular Acceleration should be increasing."; + EXPECT_LT(worldAngAccs[i - 1].Length(), worldAngAccs[i].Length()) + << "World Angular Acceleration should be increasing."; + + // --- ANG VEL (WORLD, LOCAL) + EXPECT_LT(angVels[i - 1].Length(), angVels[i].Length()) + << "Local Angular Velocity should be increasing."; + EXPECT_LT(worldAngVels[i - 1].Length(), worldAngVels[i].Length()) + << "World Angular Velocity should be increasing."; + } +} + +////////////////////////////////////////////////// +/// Tests that joints inside are supported by computing the position of +/// a pendulum bob from the joint angle. This also tests that commands such as +/// JointPositionReset work as expected. +TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(JointsInWorld)) +{ + ServerConfig serverConfig; + + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", + "joints_in_world.sdf"); + + serverConfig.SetSdfFile(sdfFile); + Server server(serverConfig); + server.SetUpdatePeriod(1ns); + + const int kIters = 1000; + test::Relay testSystem; + testSystem.OnPreUpdate( + [&](const UpdateInfo &_info, EntityComponentManager &_ecm) + { + _ecm.EachNew( + [&](const Entity &_entity, const components::Joint *, + const components::ParentEntity *) -> bool + { + enableComponent(_ecm, _entity); + enableComponent(_ecm, _entity); + return true; + }); + + if (_info.iterations == kIters / 2) + { + // After half the iterations, reset the joint position and velocity so + // that the bob at its equilibrium point and at rest. + auto jointEntity = _ecm.EntityByComponents(components::Name("j1"), + components::Joint()); + ASSERT_NE(jointEntity, kNullEntity); + _ecm.SetComponentData(jointEntity, + {0}); + _ecm.SetComponentData(jointEntity, + {GZ_PI_2}); + } + }); + testSystem.OnPostUpdate( + [&](const UpdateInfo &_info, const EntityComponentManager &_ecm) + { + auto jointEntity = _ecm.EntityByComponents(components::Name("j1"), + components::Joint()); + ASSERT_NE(jointEntity, kNullEntity); + + auto m2Entity = _ecm.EntityByComponents(components::Name("m2"), + components::Model()); + ASSERT_NE(m2Entity, kNullEntity); + + math::Pose3d m2Pose = worldPose(m2Entity, _ecm); + + auto jointPos = + _ecm.ComponentData(jointEntity); + ASSERT_TRUE(jointPos.has_value()); + ASSERT_EQ(1u, jointPos->size()); + + if (_info.iterations < kIters / 2) + { + // If the joint is properly working, the position of the model can be + // determined from the joint angle with the equations: + // x = L*cos(θ) + // z = -L*sin(θ) + // where L is the length of the model from the pivot (2m for this + // test). + const double theta = (*jointPos)[0]; + const double kLength = 2.0; + EXPECT_NEAR(m2Pose.X(), kLength * cos(theta), 1e-2); + EXPECT_NEAR(m2Pose.Z(), -kLength * sin(theta), 1e-2); + } + else + { + EXPECT_NEAR((*jointPos)[0], GZ_PI_2, 1e-2); + } + }); + + server.AddSystem(testSystem.systemPtr); + server.Run(true, 1000, false); +} diff --git a/test/integration/projector.cc b/test/integration/projector.cc new file mode 100644 index 0000000000..55fdf1c1b7 --- /dev/null +++ b/test/integration/projector.cc @@ -0,0 +1,105 @@ +/* + * 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 "gz/sim/Entity.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Projector.hh" +#include "test_config.hh" + +#include "helpers/EnvTestFixture.hh" +#include "helpers/Relay.hh" + +using namespace gz; +using namespace sim; + +class ProjectorTest : public InternalFixture<::testing::Test> +{ + public: void LoadWorld(const std::string &_path, bool _useLevels = false) + { + this->serverConfig.SetSdfFile( + common::joinPaths(PROJECT_SOURCE_PATH, _path)); + this->serverConfig.SetUseLevels(_useLevels); + + this->server = std::make_unique(this->serverConfig); + EXPECT_FALSE(this->server->Running()); + EXPECT_FALSE(*this->server->Running(0)); + using namespace std::chrono_literals; + this->server->SetUpdatePeriod(1ns); + } + + public: ServerConfig serverConfig; + public: std::unique_ptr server; +}; + +///////////////////////////////////////////////// +// Load an SDF with a projector and verify its properties. +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(ProjectorTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDFLoad)) +{ + bool projectorChecked{false}; + + this->LoadWorld(common::joinPaths("test", "worlds", "projector.sdf")); + + // Create a system that checks a projector. + test::Relay testSystem; + testSystem.OnPostUpdate([&](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const gz::sim::Entity &, + const components::Projector *_projector, + const components::Name *_name, + const components::Pose *_pose) -> bool + { + if (_name->Data() == "projector") + { + projectorChecked = true; + + EXPECT_EQ("projector", _name->Data()); + EXPECT_EQ(_name->Data(), _projector->Data().Name()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), _pose->Data()); + EXPECT_EQ(_pose->Data(), _projector->Data().RawPose()); + EXPECT_DOUBLE_EQ(2.0, _projector->Data().NearClip()); + EXPECT_DOUBLE_EQ(7.0, _projector->Data().FarClip()); + EXPECT_EQ(math::Angle(0.5), _projector->Data().HorizontalFov()); + EXPECT_EQ(0x01, _projector->Data().VisibilityFlags()); + EXPECT_EQ(common::joinPaths("path", "to", "dummy_image.png"), + _projector->Data().Texture()); + } + return true; + }); + }); + + this->server->AddSystem(testSystem.systemPtr); + this->server->Run(true, 1, false); + + EXPECT_TRUE(projectorChecked); +} diff --git a/test/integration/reset_sensors.cc b/test/integration/reset_sensors.cc index 6a0adbc6a2..592f918380 100644 --- a/test/integration/reset_sensors.cc +++ b/test/integration/reset_sensors.cc @@ -201,7 +201,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) // Run until a sensor measurement pressureReceiver.Start(topic); imageReceiver.Start("camera"); - while (!(pressureReceiver.msgReceived && imageReceiver.msgReceived)) + while (!pressureReceiver.msgReceived) { // Step once to get sensor to output measurement server.Run(true, 1, false); @@ -210,6 +210,12 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_GE(server.IterationCount().value(), current); EXPECT_FLOAT_EQ(kStartingPressure, pressureReceiver.Last().pressure()); + while (!imageReceiver.msgReceived) + { + // Step once to get sensor to output measurement + server.Run(true, 1, false); + } + // Mostly green box { auto image = toImage(imageReceiver.Last()); @@ -218,7 +224,6 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_FLOAT_EQ(0.0, centerPix.R()); EXPECT_FLOAT_EQ(0.0, centerPix.B()); } - // Run until 2000 steps pressureReceiver.msgReceived = false; imageReceiver.msgReceived = false; @@ -284,7 +289,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) current = 2001; target = 4001; - while (!(pressureReceiver.msgReceived && imageReceiver.msgReceived)) + while (!pressureReceiver.msgReceived) { // Step once to get sensor to output measurement server.Run(true, 1, false); @@ -292,6 +297,12 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) EXPECT_GE(server.IterationCount().value(), 1u); EXPECT_FLOAT_EQ(kStartingPressure, pressureReceiver.Last().pressure()); + while (!imageReceiver.msgReceived) + { + // Step once to get sensor to output measurement + server.Run(true, 1, false); + } + // Mostly green box { auto image = toImage(imageReceiver.Last()); @@ -304,6 +315,7 @@ TEST_F(ResetFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleReset)) // Run until target steps pressureReceiver.msgReceived = false; imageReceiver.msgReceived = false; + server.Run(true, 2000 - server.IterationCount().value(), false); // Check iterator state diff --git a/test/integration/rf_comms.cc b/test/integration/rf_comms.cc index 38c191a7ed..619c994c87 100644 --- a/test/integration/rf_comms.cc +++ b/test/integration/rf_comms.cc @@ -63,12 +63,11 @@ TEST_F(RFCommsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) { // Verify msg content std::lock_guard lock(mutex); - std::string expected = "hello world " + std::to_string(msgCounter); gz::msgs::StringMsg receivedMsg; receivedMsg.ParseFromString(_msg.data()); - EXPECT_EQ(expected, receivedMsg.data()); + EXPECT_NE(std::string::npos, receivedMsg.data().find("hello world")); ASSERT_GT(_msg.header().data_size(), 0); EXPECT_EQ("rssi", _msg.header().data(0).key()); msgCounter++; @@ -108,14 +107,16 @@ TEST_F(RFCommsTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(RFComms)) server.Run(true, 100, false); } - // Verify subscriber received all msgs. + // there is a non-zero probability that the packet may be lost + // Verify subscriber received most of the msgs. + unsigned int expectedMsgCount = static_cast(pubCount * 0.5); int sleep = 0; bool done = false; while (!done && sleep++ < 10) { std::lock_guard lock(mutex); - done = msgCounter == pubCount; + done = msgCounter > expectedMsgCount; std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - EXPECT_EQ(pubCount, msgCounter); + EXPECT_LT(expectedMsgCount, msgCounter); } diff --git a/test/integration/scene_broadcaster_system.cc b/test/integration/scene_broadcaster_system.cc index 278967d0d3..0ea532d4e1 100644 --- a/test/integration/scene_broadcaster_system.cc +++ b/test/integration/scene_broadcaster_system.cc @@ -1030,6 +1030,66 @@ TEST_P(SceneBroadcasterTest, EXPECT_EQ(1, count); } +TEST_P(SceneBroadcasterTest, + GZ_UTILS_TEST_DISABLED_ON_WIN32(SceneInfoHasProjector)) +{ + // Start server + gz::sim::ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "projector.sdf")); + + sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Run server + server.Run(true, 1, false); + + // Create requester + transport::Node node; + + bool result{false}; + unsigned int timeout{5000}; + gz::msgs::Scene res; + + EXPECT_TRUE(node.Request("/world/projectors/scene/info", + timeout, res, result)); + ASSERT_TRUE(result); + + ASSERT_EQ(2, res.model_size()); + int count = 0; + for (int i = 0; i < res.model_size(); ++i) + { + if (res.model(i).name() == "projector_model") + { + count++; + // There should be one link + ASSERT_EQ(1, res.model(i).link_size()); + // The link should have one projector + ASSERT_EQ(1, res.model(i).link(0).projector_size()); + + // Check a few parameter values to make sure we have the correct + // projector + const msgs::Projector &projector = + res.model(i).link(0).projector(0); + EXPECT_EQ("projector", projector.name()); + EXPECT_EQ(math::Pose3d(0, 1, 0, 0, 0, 0), + msgs::Convert(projector.pose())); + EXPECT_DOUBLE_EQ(2.0, projector.near_clip()); + EXPECT_DOUBLE_EQ(7.0, projector.far_clip()); + EXPECT_DOUBLE_EQ(0.5, projector.fov()); + EXPECT_NE(std::string::npos, + projector.texture().find("path/to/dummy_image.png")); + auto header = projector.header().data(0); + EXPECT_EQ("visibility_flags", header.key()); + EXPECT_EQ(0x01, std::stoul(header.value(0))); + } + } + + // Should have found 1 projector + EXPECT_EQ(1, count); +} + // Run multiple times INSTANTIATE_TEST_SUITE_P(ServerRepeat, SceneBroadcasterTest, ::testing::Range(1, 2)); diff --git a/test/integration/sensor.cc b/test/integration/sensor.cc new file mode 100644 index 0000000000..b535080aeb --- /dev/null +++ b/test/integration/sensor.cc @@ -0,0 +1,171 @@ +/* + * 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 +#include +#include +#include +#include + +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +class SensorIntegrationTest : public InternalFixture<::testing::Test> +{ +}; + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Valid) +{ + EntityComponentManager ecm; + + // No ID + { + Sensor sensor; + EXPECT_FALSE(sensor.Valid(ecm)); + } + + // Missing sensor component + { + auto id = ecm.CreateEntity(); + Sensor sensor(id); + EXPECT_FALSE(sensor.Valid(ecm)); + } + + // Valid + { + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Sensor()); + + Sensor sensor(id); + EXPECT_TRUE(sensor.Valid(ecm)); + } +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Name) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Sensor()); + + Sensor sensor(id); + + // No name + EXPECT_EQ(std::nullopt, sensor.Name(ecm)); + + // Add name + ecm.CreateComponent(id, components::Name("sensor_name")); + EXPECT_EQ("sensor_name", sensor.Name(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Pose) +{ + EntityComponentManager ecm; + + auto id = ecm.CreateEntity(); + ecm.CreateComponent(id, components::Sensor()); + + Sensor sensor(id); + + // No pose + EXPECT_EQ(std::nullopt, sensor.Pose(ecm)); + + // Add pose + math::Pose3d pose(1, 2, 3, 0.1, 0.2, 0.3); + ecm.CreateComponent(id, + components::Pose(pose)); + EXPECT_EQ(pose, sensor.Pose(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Topic) +{ + EntityComponentManager ecm; + + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + + Sensor sensor(eSensor); + + std::string topic = "sensor_topic"; + ecm.CreateComponent(eSensor, + components::SensorTopic(topic)); + + EXPECT_EQ(topic, sensor.Topic(ecm)); +} + +////////////////////////////////////////////////// +TEST_F(SensorIntegrationTest, Parent) +{ + EntityComponentManager ecm; + + { + // Link as parent + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + + Sensor sensor(eSensor); + EXPECT_EQ(eSensor, sensor.Entity()); + EXPECT_FALSE(sensor.Parent(ecm).has_value()); + + ecm.CreateComponent(eSensor, + components::ParentEntity(eLink)); + ASSERT_TRUE(sensor.Valid(ecm)); + + // Check parent link + EXPECT_EQ(eLink, ecm.ParentEntity(eSensor)); + auto parentLink = sensor.Parent(ecm); + EXPECT_EQ(eLink, parentLink); + } + + { + // Joint as parent + auto eJoint = ecm.CreateEntity(); + ecm.CreateComponent(eJoint, components::Joint()); + auto eSensor = ecm.CreateEntity(); + ecm.CreateComponent(eSensor, components::Sensor()); + + Sensor sensor(eSensor); + EXPECT_EQ(eSensor, sensor.Entity()); + EXPECT_FALSE(sensor.Parent(ecm).has_value()); + + ecm.CreateComponent(eSensor, + components::ParentEntity(eJoint)); + ASSERT_TRUE(sensor.Valid(ecm)); + + // Check parent joint + EXPECT_EQ(eJoint, ecm.ParentEntity(eSensor)); + auto parentJoint = sensor.Parent(ecm); + EXPECT_EQ(eJoint, parentJoint); + } +} diff --git a/test/integration/sensors_system.cc b/test/integration/sensors_system.cc index 54ab56ba6b..84e95e94f6 100644 --- a/test/integration/sensors_system.cc +++ b/test/integration/sensors_system.cc @@ -93,6 +93,39 @@ void testSensorEntityIds(const sim::EntityComponentManager &_ecm, } } +///////////////////////////////////////////////// +void testSensorTopicComponents(const sim::EntityComponentManager &_ecm, + const std::unordered_set &_ids, + const std::vector &_topics) +{ + EXPECT_FALSE(_ids.empty()); + for (const auto & id : _ids) + { + auto sensorTopicComp = _ecm.Component(id); + EXPECT_TRUE(sensorTopicComp); + std::string topicStr = "/" + sensorTopicComp->Data(); + EXPECT_FALSE(topicStr.empty()); + + // verify that the topic string stored in sensor topic component + // exits in the list of topics + // For rendering sensors, they may advertize more than one topics but + // the the sensor topic component will only contain one of them, e.g. + // * /image - stored in sensor topic component + // * /camera_info + bool foundTopic = false; + for (auto it = _topics.begin(); it != _topics.end(); ++it) + { + std::string topic = *it; + if (topic.find(topicStr) == 0u) + { + foundTopic = true; + break; + } + } + EXPECT_TRUE(foundTopic); + } +} + ////////////////////////////////////////////////// class SensorsFixture : public InternalFixture> { @@ -117,21 +150,10 @@ class SensorsFixture : public InternalFixture> }; ////////////////////////////////////////////////// -void testDefaultTopics() +void testDefaultTopics(const std::vector &_topics) { // TODO(anyone) This should be a new test, but running multiple tests with // sensors is not currently working - std::string prefix{"/world/camera_sensor/model/default_topics/"}; - std::vector topics{ - prefix + "link/camera_link/sensor/camera/image", - prefix + "link/camera_link/sensor/camera/camera_info", - prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", - prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", - prefix + "link/depth_camera_link/sensor/depth_camera/camera_info", - prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", - prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image" - }; - std::vector publishers; std::vector subscribers; transport::Node node; @@ -141,14 +163,14 @@ void testDefaultTopics() int sleep{0}; int maxSleep{30}; for (; sleep < maxSleep && - !node.TopicInfo(topics.front(), publishers, subscribers); + !node.TopicInfo(_topics.front(), publishers, subscribers); ++sleep) { std::this_thread::sleep_for(std::chrono::milliseconds(100)); } ASSERT_LT(sleep, maxSleep); - for (const std::string &topic : topics) + for (const std::string &topic : _topics) { bool result = node.TopicInfo(topic, publishers, subscribers); @@ -200,9 +222,30 @@ TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(HandleRemovedEntities)) server.Run(true, 50, false); ASSERT_NE(nullptr, ecm); - testDefaultTopics(); + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + std::vector topics{ + prefix + "link/camera_link/sensor/camera/image", + prefix + "link/camera_link/sensor/camera/camera_info", + prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + prefix + "link/depth_camera_link/sensor/depth_camera/camera_info", + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/depth_image", + prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/labels_map", + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/camera_info", + "/camera" + }; + testDefaultTopics(topics); testSensorEntityIds(*ecm, g_sensorEntityIds); + testSensorTopicComponents(*ecm, g_sensorEntityIds, topics); + g_sensorEntityIds.clear(); g_scene.reset(); diff --git a/test/integration/sensors_system_update_rate.cc b/test/integration/sensors_system_update_rate.cc new file mode 100644 index 0000000000..8cd5d802df --- /dev/null +++ b/test/integration/sensors_system_update_rate.cc @@ -0,0 +1,311 @@ +/* + * 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 "gz/sim/components/Camera.hh" +#include "gz/sim/components/DepthCamera.hh" +#include "gz/sim/components/GpuLidar.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Physics.hh" +#include "gz/sim/components/RgbdCamera.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/SegmentationCamera.hh" +#include "gz/sim/components/ThermalCamera.hh" +#include "gz/sim/components/World.hh" + +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" +#include "test_config.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace std::chrono_literals; +namespace components = gz::sim::components; + +////////////////////////////////////////////////// +class SensorsFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("gz::sim::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: gz::sim::SystemPluginPtr systemPtr; + public: sim::MockSystem *mockSystem; + + private: sim::SystemLoader sm; +}; + +///////////////////////////////////////////////// +TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(UpdateRate)) +{ + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/sensor.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + sim::Server server(serverConfig); + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + this->mockSystem->configureCallback = + [&](const sim::Entity &, + const std::shared_ptr &, + sim::EntityComponentManager &_ecm, + sim::EventManager &) + { + ecm = &_ecm; + }; + + server.AddSystem(this->systemPtr); + transport::Node node; + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + + std::vector imageTimestamps; + unsigned int imageCount = 0u; + auto cameraCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + imageTimestamps.push_back(t); + ++imageCount; + }); + node.Subscribe(prefix + "link/camera_link/sensor/camera/image", cameraCb); + + std::vector lidarTimestamps; + unsigned int lidarCount = 0u; + auto lidarCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + lidarTimestamps.push_back(t); + ++lidarCount; + }); + node.Subscribe(prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", lidarCb); + + std::vector depthTimestamps; + unsigned int depthCount = 0u; + auto depthCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + depthTimestamps.push_back(t); + ++depthCount; + }); + node.Subscribe( + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + depthCb); + + std::vector rgbdTimestamps; + unsigned int rgbdCount = 0u; + auto rgbdCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + rgbdTimestamps.push_back(t); + ++rgbdCount; + }); + node.Subscribe( + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + rgbdCb); + + std::vector thermalTimestamps; + unsigned int thermalCount = 0u; + auto thermalCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + thermalTimestamps.push_back(t); + ++thermalCount; + }); + node.Subscribe( + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + thermalCb); + + std::vector segmentationTimestamps; + unsigned int segmentationCount = 0u; + auto segmentationCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + segmentationTimestamps.push_back(t); + ++segmentationCount; + }); + node.Subscribe( + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + segmentationCb); + + unsigned int iterations = 2000u; + server.Run(true, iterations, false); + + EXPECT_NE(nullptr, ecm); + + // get the world step size + auto worldEntity = ecm->EntityByComponents(components::World()); + EXPECT_NE(sim::kNullEntity, worldEntity); + auto physicsSdf = ecm->Component(worldEntity)->Data(); + double stepSize = physicsSdf.MaxStepSize(); + EXPECT_LT(0, stepSize); + + // get the sensors update rates + auto camLinkEntity = ecm->EntityByComponents(components::Name("camera_link")); + EXPECT_NE(sim::kNullEntity, camLinkEntity); + auto camEntity = ecm->EntityByComponents(components::Name("camera"), + components::ParentEntity(camLinkEntity)); + EXPECT_NE(sim::kNullEntity, camEntity); + auto sensorSdf = ecm->Component(camEntity)->Data(); + unsigned int camRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, camRate); + + auto lidarEntity = ecm->EntityByComponents(components::Name("gpu_lidar")); + EXPECT_NE(sim::kNullEntity, lidarEntity); + sensorSdf = ecm->Component(lidarEntity)->Data(); + unsigned int lidarRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, lidarRate); + + auto depthEntity = ecm->EntityByComponents(components::Name("depth_camera")); + EXPECT_NE(sim::kNullEntity, depthEntity); + sensorSdf = ecm->Component(depthEntity)->Data(); + unsigned int depthRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, depthRate); + + auto rgbdEntity = ecm->EntityByComponents(components::Name("rgbd_camera")); + EXPECT_NE(sim::kNullEntity, rgbdEntity); + sensorSdf = ecm->Component(rgbdEntity)->Data(); + unsigned int rgbdRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, rgbdRate); + + auto thermalEntity = ecm->EntityByComponents( + components::Name("thermal_camera")); + EXPECT_NE(sim::kNullEntity, thermalEntity); + sensorSdf = ecm->Component(thermalEntity)->Data(); + unsigned int thermalRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, thermalRate); + + auto segmentationEntity = ecm->EntityByComponents( + components::Name("segmentation_camera")); + EXPECT_NE(sim::kNullEntity, segmentationEntity); + sensorSdf = ecm->Component( + segmentationEntity)->Data(); + unsigned int segmentationRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, segmentationRate); + + // compute and verify expected msg count based on update rate and sim time + double timeRan = iterations * stepSize; + + unsigned int expectedCamMsgCount = timeRan / (1.0 / camRate) + 1; + unsigned int expectedDepthMsgCount = timeRan / (1.0 / depthRate) + 1; + unsigned int expectedLidarMsgCount = timeRan / (1.0 / lidarRate) + 1; + unsigned int expectedRgbdMsgCount = timeRan / (1.0 / rgbdRate) + 1; + unsigned int expectedThermalMsgCount = timeRan / (1.0 / thermalRate) + 1; + unsigned int expectedSegmentationMsgCount = + timeRan / (1.0 / segmentationRate) + 1; + + unsigned int sleep = 0; + unsigned int maxSleep = 100; + while (sleep++ < maxSleep && + (imageCount < expectedCamMsgCount || + lidarCount < expectedLidarMsgCount || + depthCount < expectedDepthMsgCount || + rgbdCount < expectedRgbdMsgCount || + thermalCount < expectedThermalMsgCount || + segmentationCount < expectedSegmentationMsgCount)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_EQ(expectedCamMsgCount, imageCount); + EXPECT_EQ(expectedDepthMsgCount, depthCount); + EXPECT_EQ(expectedLidarMsgCount, lidarCount); + EXPECT_EQ(expectedRgbdMsgCount, rgbdCount); + EXPECT_EQ(expectedThermalMsgCount, thermalCount); + EXPECT_EQ(expectedSegmentationMsgCount, segmentationCount); + + // verify timestamps + // The first timestamp may not be 0 because the rendering + // may take some time to start and it does not block the main thread + // so start with index = 1 + // \todo(anyone) Make the sensors system thread block so we always + // generate data at t = 0? + EXPECT_FALSE(imageTimestamps.empty()); + EXPECT_FALSE(lidarTimestamps.empty()); + EXPECT_FALSE(depthTimestamps.empty()); + EXPECT_FALSE(rgbdTimestamps.empty()); + EXPECT_FALSE(thermalTimestamps.empty()); + EXPECT_FALSE(segmentationTimestamps.empty()); + for (unsigned int i = 1; i < imageTimestamps.size()-1; ++i) + { + double dt = imageTimestamps[i+1] - imageTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / camRate, dt); + } + for (unsigned int i = 1; i < lidarTimestamps.size()-1; ++i) + { + double dt = lidarTimestamps[i+1] - lidarTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / lidarRate, dt); + } + for (unsigned int i = 1; i < depthTimestamps.size()-1; ++i) + { + double dt = depthTimestamps[i+1] - depthTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / depthRate, dt); + } + for (unsigned int i = 1; i < rgbdTimestamps.size()-1; ++i) + { + double dt = rgbdTimestamps[i+1] - rgbdTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / rgbdRate, dt); + } + for (unsigned int i = 1; i < thermalTimestamps.size()-1; ++i) + { + double dt = thermalTimestamps[i+1] - thermalTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / thermalRate, dt); + } + for (unsigned int i = 1; i < segmentationTimestamps.size()-1; ++i) + { + double dt = segmentationTimestamps[i+1] - segmentationTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / segmentationRate, dt); + } +} diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc index 7904f6a856..782bd2b4cd 100644 --- a/test/integration/triggered_camera.cc +++ b/test/integration/triggered_camera.cc @@ -25,7 +25,9 @@ #pragma warning(pop) #endif +#include "gz/sim/rendering/Events.hh" #include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" #include "test_config.hh" #include #include @@ -43,11 +45,30 @@ using namespace std::chrono_literals; /// \brief Test TriggeredCameraTest system class TriggeredCameraTest : public InternalFixture<::testing::Test> { + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("gz::sim::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: SystemPluginPtr systemPtr; + public: MockSystem *mockSystem; + + private: SystemLoader sm; }; std::mutex mutex; msgs::Image imageMsg; unsigned char *imageBuffer = nullptr; +bool renderingStarted = false; ///////////////////////////////////////////////// void imageCb(const msgs::Image &_msg) @@ -64,6 +85,13 @@ void imageCb(const msgs::Image &_msg) mutex.unlock(); } +///////////////////////////////////////////////// +void OnPostRender() +{ + std::lock_guard lock(mutex); + renderingStarted = true; +} + ///////////////////////////////////////////////// // The test checks the Triggered Camera readings TEST_F(TriggeredCameraTest, @@ -80,8 +108,34 @@ TEST_F(TriggeredCameraTest, EXPECT_FALSE(*server.Running(0)); server.SetUpdatePeriod(100us); + + common::ConnectionPtr postRenderConn; + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + this->mockSystem->configureCallback = + [&](const Entity &, + const std::shared_ptr &, + EntityComponentManager &, + EventManager &_eventMgr) + { + postRenderConn = _eventMgr.Connect( + std::bind(&::OnPostRender)); + }; + + server.AddSystem(this->systemPtr); server.Run(false, 0, false); + // wait for rendering to be initialized + int sleep{0}; + int maxSleep{30}; + bool ready = false; + while (!ready && sleep++ < maxSleep) + { + std::this_thread::sleep_for(100ms); + std::lock_guard lock(mutex); + ready = renderingStarted; + } + // Subscribe to the image topic transport::Node node; node.Subscribe("/camera", &imageCb); @@ -94,8 +148,7 @@ TEST_F(TriggeredCameraTest, msgs::Boolean msg; msg.set_data(true); - int sleep{0}; - int maxSleep{30}; + sleep = 0; while (imageBuffer == nullptr && sleep < maxSleep) { pub.Publish(msg); diff --git a/test/performance/CMakeLists.txt b/test/performance/CMakeLists.txt index 770ba7b937..81cbd25f75 100644 --- a/test/performance/CMakeLists.txt +++ b/test/performance/CMakeLists.txt @@ -5,6 +5,21 @@ set(tests level_manager.cc ) +set(tests_needing_display + sensors_system.cc +) + +# Add systems that need a valid display here. +if(VALID_DISPLAY AND VALID_DRI_DISPLAY) + list(APPEND tests ${tests_needing_display}) +else() + message(STATUS + "Skipping these PERFORMANCE tests because a valid display was not found:") + foreach(test ${tests_needing_display}) + message(STATUS " ${test}") + endforeach(test) +endif() + set(exec sdf_runner ) @@ -39,3 +54,9 @@ target_link_libraries( gz-sim${PROJECT_VERSION_MAJOR} gz-sim${PROJECT_VERSION_MAJOR}-gui ) + +if(VALID_DISPLAY AND VALID_DRI_DISPLAY AND TARGET PERFORMANCE_sensors_system) + target_link_libraries(PERFORMANCE_sensors_system + gz-rendering${GZ_RENDERING_VER}::gz-rendering${GZ_RENDERING_VER} + ) +endif() diff --git a/test/performance/sensors_system.cc b/test/performance/sensors_system.cc new file mode 100644 index 0000000000..86e8ee3356 --- /dev/null +++ b/test/performance/sensors_system.cc @@ -0,0 +1,136 @@ +/* + * 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. + * +*/ + +// The following is needed to enable the GetMemInfo function for OSX +#ifdef __MACH__ +# include +#endif // __MACH__ + +#include + +#include +#include + +#include + +#include "gz/sim/Server.hh" +#include "test_config.hh" + +#include "../helpers/EnvTestFixture.hh" + +///////////////////////////////////////////////// +void getMemInfo(double &_resident, double &_share) +{ +#ifdef __linux__ + int totalSize, residentPages, sharePages; + totalSize = residentPages = sharePages = 0; + + std::ifstream buffer("/proc/self/statm"); + buffer >> totalSize >> residentPages >> sharePages; + buffer.close(); + + // in case x86-64 is configured to use 2MB pages + int64_t pageSizeKb = sysconf(_SC_PAGE_SIZE) / 1024; + + _resident = residentPages * pageSizeKb; + _share = sharePages * pageSizeKb; +#elif __MACH__ + // /proc is only available on Linux + // for OSX, use task_info to get resident and virtual memory + struct task_basic_info t_info; + mach_msg_type_number_t t_info_count = TASK_BASIC_INFO_COUNT; + if (KERN_SUCCESS != task_info(mach_task_self(), + TASK_BASIC_INFO, + (task_info_t)&t_info, + &t_info_count)) + { + gzerr << "failure calling task_info\n"; + return; + } + _resident = static_cast(t_info.resident_size/1024); + _share = static_cast(t_info.virtual_size/1024); +#else + gzerr << "Unsupported architecture\n"; + return; +#endif +} + +///////////////////////////////////////////////// +class SensorsSystemFixture : public InternalFixture<::testing::Test> +{ +}; + + +///////////////////////////////////////////////// +// Test repeatedly launching sim with sensors system and check for +// memory leaks +TEST_F(SensorsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(MemLeak)) +{ + gz::common::Console::SetVerbosity(4); + + // max memory change allowed + const double resMaxPercentChange = 1.0; + const double shareMaxPercentChange = 1.0; + + std::vector residentMemV; + std::vector shareMemV; + + for (unsigned int i = 0; i < 5; ++i) + { + // get resident and shared memory usage + double residentMem = 0; + double shareMem = 0; + getMemInfo(residentMem, shareMem); + + residentMemV.push_back(residentMem); + shareMemV.push_back(shareMem); + + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + + "/test/worlds/sensors_system_mem_leak.sdf"; + + serverConfig.SetSdfFile(sdfFile); + + gz::sim::Server server(serverConfig); + server.Run(true, 100, false); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + + // Calculate the percent change between runs + // Start from index 1 (instead of 0) since there is a big jump in memory usage + // when the system is first loaded. Make sure subsequent runs do not continue + // to increase memory usage + for (unsigned int i = 1; i < residentMemV.size() - 1; ++i) + { + double resPercentChange = + (residentMemV[i+1] - residentMemV[i]) / residentMemV[i]; + double sharePercentChange = (shareMemV[i+1] - shareMemV[i]) / shareMemV[i]; + + gzdbg << "ResPercentChange[" << resPercentChange << "] " + << "ResMaxPercentChange[" << resMaxPercentChange << "]" << std::endl; + gzdbg << "SharePercentChange[" << sharePercentChange << "] " + << "ShareMaxPercentChange[" << shareMaxPercentChange << "]" << std::endl; + + // check for memory leaks + // \todo(anyone) there is still gradual slow increase in memory usage + // between runs, likely from the sensors system / gz-rendering. + // Use tighter max percentage change once more mem leaks are fixed + EXPECT_LT(resPercentChange, resMaxPercentChange); + EXPECT_LT(sharePercentChange, shareMaxPercentChange); + } +} diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt index dbc095a2c7..a522517795 100644 --- a/test/worlds/CMakeLists.txt +++ b/test/worlds/CMakeLists.txt @@ -5,3 +5,4 @@ configure_file (heightmap.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/heightmap.sdf configure_file (environmental_data.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_data.sdf) configure_file (environmental_sensor.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_sensor.sdf) +configure_file (hydrodynamics.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/hydrodynamics.sdf) diff --git a/test/worlds/ackermann_steering_only.sdf b/test/worlds/ackermann_steering_only.sdf new file mode 100644 index 0000000000..b41297a888 --- /dev/null +++ b/test/worlds/ackermann_steering_only.sdf @@ -0,0 +1,374 @@ + + + + + + 0.001 + 0 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + 1 + 1 + 0.1 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 1 1 1 + 1 1 1 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 1 1 1 + 1 1 1 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_steering_joint + front_right_wheel_steering_joint + true + 10.0 + 0.5 + 1.0 + 1.25 + + + + diff --git a/test/worlds/ackermann_steering_only_actuators.sdf b/test/worlds/ackermann_steering_only_actuators.sdf new file mode 100644 index 0000000000..6b5f9232b5 --- /dev/null +++ b/test/worlds/ackermann_steering_only_actuators.sdf @@ -0,0 +1,376 @@ + + + + + + 0.001 + 0 + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + 1 + 1 + 0.1 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 1 1 1 + 1 1 1 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 1 1 1 + 1 1 1 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_steering_joint + front_right_wheel_steering_joint + true + true + 0 + 10.0 + 0.5 + 1.0 + 1.25 + + + + diff --git a/test/worlds/ackermann_steering_only_custom_sub_topics.sdf b/test/worlds/ackermann_steering_only_custom_sub_topics.sdf new file mode 100644 index 0000000000..c3685a2b6b --- /dev/null +++ b/test/worlds/ackermann_steering_only_custom_sub_topics.sdf @@ -0,0 +1,439 @@ + + + + + + 0.001 + 0 + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + 50 + + + 1 + 1 + 0.1 + + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.325 0 -0 0 + + + -0.151427 -0 0.175 0 -0 0 + + 1.14395 + + 0.126164 + 0 + 0 + 0.416519 + 0 + 0.481014 + + + + + + 2.01142 1 0.568726 + + + + 0.5 0.5 1.0 1 + 0.5 0.5 1.0 1 + 0.0 0.0 1.0 1 + + + + + + 2.01142 1 0.568726 + + + + + + + 0.554283 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + 0.5 + 1 + 0.1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + 0.5 + 1 + 0.1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + 0.5 + 1 + 0.1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.15 + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.15 + 0.3 + + + + + + 0.5 + 1.0 + 0 0 1 + + + 0.5 + 1 + 0.1 + + + + + + + + 0.554283 0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 1 1 1 + 1 1 1 + + + + + + 0.554283 -0.5 0.02 0 0 0 + + 0.5 + + 0.0153 + 0.025 + 0.0153 + + + + 0 0 0 0 0 0 + + + 0.1 + 0.03 + + + + 1 1 1 + 1 1 1 + + + + + + front_left_wheel_steering_link + chassis + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + 1 + + + + + chassis + front_right_wheel_steering_link + + 0 0 1 + + -0.6 + +0.6 + 1.0 + 25 + + + + + + front_left_wheel_steering_link + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_right_wheel_steering_link + front_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + rear_right_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + front_left_wheel_steering_joint + front_right_wheel_steering_joint + 0.5 + 1.0 + 1.25 + steerangle + + + + + + diff --git a/test/worlds/actor_trajectory.sdf b/test/worlds/actor_trajectory.sdf new file mode 100644 index 0000000000..c5631c7f23 --- /dev/null +++ b/test/worlds/actor_trajectory.sdf @@ -0,0 +1,83 @@ + + + + + + + ogre2 + + + + + + + + + .2 .2 .2 + + + + + + + + + + true + -5 0 0 0 0.5 0 + + + + + 0.1 0.1 0.1 + + + + + 0 0 0.0 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + 30 + camera + + + + + + diff --git a/test/worlds/added_mass_ellipsoids.sdf b/test/worlds/added_mass_ellipsoids.sdf new file mode 100644 index 0000000000..1c7a0131b4 --- /dev/null +++ b/test/worlds/added_mass_ellipsoids.sdf @@ -0,0 +1,126 @@ + + + + + + + + + + 0 0 0 + + + + 0 0 0 0 0 0 + + + 1073.3774899765126 + + 107.33774899765126 + 0 + 0 + 268.34437249412815 + 0 + 268.34437249412815 + + + 236.14304779483277 + 901.6370915802705 + 901.6370915802705 + 64.40264939859075 + 64.40264939859075 + + + + + + + + 0 0 0 0 0 0 + + + 1073.3774899765126 + + 107.33774899765126 + 0 + 0 + 268.34437249412815 + 0 + 268.34437249412815 + + + 23.614304779483277 + 90.16370915802705 + 90.16370915802705 + 6.440264939859075 + 6.440264939859075 + + + + + + + + 0 0 0 0 0 0 + + + 1073.3774899765126 + + 107.33774899765126 + 0 + 0 + 268.34437249412815 + 0 + 268.34437249412815 + + + 2361.4304779483277 + 9016.370915802705 + 9016.370915802705 + 644.0264939859075 + 644.0264939859075 + + + + + + diff --git a/test/worlds/added_mass_full_matrix.sdf b/test/worlds/added_mass_full_matrix.sdf new file mode 100644 index 0000000000..958f8e5543 --- /dev/null +++ b/test/worlds/added_mass_full_matrix.sdf @@ -0,0 +1,54 @@ + + + + + + + + + + 0 0 0 + + + 0 0 0 0 0 0 + + + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + 0.3949679265119419 + 0.046356515552357314 + 0.10350947385435993 + 0.898797296303345 + 0.49882101032414017 + 0.37538990588775467 + 0.7251406148851146 + 0.6826397691305769 + 0.7790258092356468 + 0.3095523984784859 + 0.9762734675205801 + 0.7127177841503902 + 0.968056899449243 + 0.7054363474498657 + 0.9327424023111127 + 0.7730463848840338 + 0.5459481647643502 + 0.8074138574151712 + 0.13432180787188996 + 0.8255294272171281 + 0.2597705135260855 + + + + + + diff --git a/test/worlds/air_speed.sdf b/test/worlds/air_speed.sdf new file mode 100644 index 0000000000..feb3347655 --- /dev/null +++ b/test/worlds/air_speed.sdf @@ -0,0 +1,61 @@ + + + + + 0 + + + + + + + + + true + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 30 + true + + + + 0 + 0 + + + + + + + + + diff --git a/test/worlds/battery.sdf b/test/worlds/battery.sdf index 8fd9a826a1..007d8f536b 100644 --- a/test/worlds/battery.sdf +++ b/test/worlds/battery.sdf @@ -54,6 +54,7 @@ 1.9499 500 + true @@ -84,8 +85,8 @@ 1.9499 500 - /battery/discharge1 - /battery/discharge2 + /battery/discharge + /battery/stop_discharge diff --git a/test/worlds/battery_thruster_consumer.sdf b/test/worlds/battery_thruster_consumer.sdf new file mode 100644 index 0000000000..58afe3720c --- /dev/null +++ b/test/worlds/battery_thruster_consumer.sdf @@ -0,0 +1,220 @@ + + + + + + + 0 + + + + 0 0 0 + + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + lowbattery + propeller_joint + 0.005 + 950 + 0.25 + true + 300 + 0 + thrust + + + + linear_battery + 12.592 + 12.694 + -3.1424 + 1.2009 + 1.2009 + 0.061523 + 1.9499 + + 500 + true + + + + + + + 0 0 0 0 1.57 0 + + 100 + + 33.89 + 0 + 0 + 33.89 + 0 + 1.125 + + + + + + 2 + 0.15 + + + + + + + -1.05 0 0 0 0 0 + + 0.1 + + 0.000354167 + 0 + 0 + 0.000021667 + 0 + 0.000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + lowbattery + propeller_joint + 0.005 + 950 + 0.25 + true + 300 + 0 + thrust + 500 + linear_battery2 + + + + linear_battery2 + 12.592 + 12.694 + -3.1424 + 1.2009 + 1.2009 + 0.061523 + 1.9499 + + 500 + true + + + + diff --git a/test/worlds/environmental_sensor.csv b/test/worlds/environmental_sensor.csv index 3ec2c0a571..e5610e9c2f 100644 --- a/test/worlds/environmental_sensor.csv +++ b/test/worlds/environmental_sensor.csv @@ -1,17 +1,17 @@ -timestamp,humidity,x,y,z -0,0,-1,-1,-1 -0,0,-1,-1, 1 -0,0,-1, 1,-1 -0,0,-1, 1, 1 -0,0, 1,-1,-1 -0,0, 1,-1, 1 -0,0, 1, 1,-1 -0,0, 1, 1, 1 -1,1,-1,-1,-1 -1,1,-1,-1, 1 -1,1,-1, 1,-1 -1,1,-1, 1, 1 -1,1, 1,-1,-1 -1,1, 1,-1, 1 -1,1, 1, 1,-1 -1,1, 1, 1, 1 +timestamp,humidity,x,y,z,wind_speed_x,wind_speed_y,wind_speed_z +0,0,-1,-1,-1,1,0,0 +0,0,-1,-1, 1,1,0,0 +0,0,-1, 1,-1,1,0,0 +0,0,-1, 1, 1,1,0,0 +0,0, 1,-1,-1,1,0,0 +0,0, 1,-1, 1,1,0,0 +0,0, 1, 1,-1,1,0,0 +0,0, 1, 1, 1,1,0,0 +1,1,-1,-1,-1,1,0,0 +1,1,-1,-1, 1,1,0,0 +1,1,-1, 1,-1,1,0,0 +1,1,-1, 1, 1,1,0,0 +1,1, 1,-1,-1,1,0,0 +1,1, 1,-1, 1,1,0,0 +1,1, 1, 1,-1,1,0,0 +1,1, 1, 1, 1,1,0,0 diff --git a/test/worlds/environmental_sensor.sdf.in b/test/worlds/environmental_sensor.sdf.in index 46b3673ea7..85e0de6150 100644 --- a/test/worlds/environmental_sensor.sdf.in +++ b/test/worlds/environmental_sensor.sdf.in @@ -75,7 +75,7 @@ - 0 0 0.05 0 0 0 + 0 0 0.05 0 0 1.57 0.1 @@ -105,6 +105,30 @@ 30 true + + + + 1 + 30 + true + vector3 + wind_speed2d + wind_speed_x + wind_speed_y + + + + + 1 + 30 + true + vector3 + wind_speed3d + LOCAL + wind_speed_x + wind_speed_y + wind_speed_z + diff --git a/test/worlds/fluid_added_mass.sdf b/test/worlds/fluid_added_mass.sdf new file mode 100644 index 0000000000..072b895c95 --- /dev/null +++ b/test/worlds/fluid_added_mass.sdf @@ -0,0 +1,1005 @@ + + + + + + + + + + + + + box_xx + model + 1 0 0 + + + + box_xy + model + 1 0 0 + + + + box_xz + model + 1 0 0 + + + + box_xp + model + 1 0 0 + + + + box_xq + model + 1 0 0 + + + + box_xr + model + 1 0 0 + + + + box_yy + model + 1 0 0 + + + + box_yz + model + 1 0 0 + + + + box_yp + model + 1 0 0 + + + + box_yq + model + 1 0 0 + + + + box_yr + model + 1 0 0 + + + + box_zz + model + 1 0 0 + + + + box_zp + model + 1 0 0 + + + + box_zq + model + 1 0 0 + + + + box_zr + model + 1 0 0 + + + + box_pp + model + 1 0 0 + + + + box_pq + model + 1 0 0 + + + + box_pr + model + 1 0 0 + + + + box_qq + model + 1 0 0 + + + + box_qr + model + 1 0 0 + + + + box_rr + model + 1 0 0 + + + + + + 0 0 0 + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + + 0 0.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 2.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 4.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 6.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 8.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 10.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 12.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 14.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 16.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 18.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 20.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 22.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 24.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 26.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 28.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 30.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 32.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 34.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 36.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 38.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + 0 40.0 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + 0.261666 + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + + + + diff --git a/test/worlds/fluid_added_mass.sdf.erb b/test/worlds/fluid_added_mass.sdf.erb new file mode 100644 index 0000000000..6563f4b242 --- /dev/null +++ b/test/worlds/fluid_added_mass.sdf.erb @@ -0,0 +1,102 @@ + +<% + elems = ['xx', 'xy', 'xz', 'xp', 'xq', 'xr', + 'yy', 'yz', 'yp', 'yq', 'yr', + 'zz', 'zp', 'zq', 'zr', + 'pp', 'pq', 'pr', + 'qq', 'qr', + 'rr'] +%> + + + + + + + + +<% + elems.each do |elem| +%> + + box_<%= elem %> + model + 1 0 0 + +<% + end +%> + + + + 0 0 0 + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + +<% + pos_x = -2.0 + elems.each do |elem| + pos_x += 2.0 +%> + + 0 <%= pos_x %> 0.5 0 0 0 + + + + 0.1 + 0 + 0 + 0.1 + 0 + 0.1 + + 1.0 + + <<%= elem %>>0.261666> + + + + + + 1.0 1.0 1.0 + + + + + + + + 1.0 1.0 1.0 + + + + 0 1 1 1 + 0 1 1 1 + 1 0 1 1 + + + + +<% + end +%> + + diff --git a/test/worlds/hydrodynamic_currents.csv b/test/worlds/hydrodynamic_currents.csv new file mode 100644 index 0000000000..aa967fc688 --- /dev/null +++ b/test/worlds/hydrodynamic_currents.csv @@ -0,0 +1,17 @@ +timestamp,x,y,z,current_x,current_y,current_z +0,-20,-20, 20,1,0,0 +0,20,-20, 20,1,0,0 +0,-20,-20,-20,1,0,0 +0,20,-20,-20,1,0,0 +0,-20, 20, 20,1,0,0 +0,20, 20, 20,1,0,0 +0,-20, 20,-20,1,0,0 +0,20, 20,-20,1,0,0 +10000, 20,-20, 20,1,0,0 +10000,-20,-20, 20,1,0,0 +10000, 20,-20,-20,1,0,0 +10000,-20,-20,-20,1,0,0 +10000, 20, 20, 20,1,0,0 +10000,-20, 20, 20,1,0,0 +10000, 20, 20,-20,1,0,0 +10000,-20, 20,-20,1,0,0 diff --git a/test/worlds/hydrodynamics.sdf b/test/worlds/hydrodynamics.sdf.in similarity index 69% rename from test/worlds/hydrodynamics.sdf rename to test/worlds/hydrodynamics.sdf.in index 7fc4790c84..7e86144be7 100644 --- a/test/worlds/hydrodynamics.sdf +++ b/test/worlds/hydrodynamics.sdf.in @@ -11,9 +11,24 @@ 0 0 0 + + + @CMAKE_SOURCE_DIR@/test/worlds/hydrodynamic_currents.csv + + + + x + y + z + + + + + name="gz::sim::systems::Physics"> @@ -94,8 +109,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> sphere2_link 918 0 @@ -104,17 +119,17 @@ 0 0 0 - 0 + 0 0 - 0 + 0 0 - 11.5359 + 11.5359 0.211869 - 0 + 0 0 - 0 + 0 0 - 0 + 0 0 @@ -153,8 +168,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> cylinder1_link 1000 0 @@ -163,17 +178,17 @@ 0 0 0 - 0 + 0 0 - 0 + 0 0 - 94.2475 + 94.2475 0.0037699 - 0 + 0 0 - 0 + 0 0 - 0 + 0 0 @@ -212,8 +227,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> cylinder2_link 1000 0 @@ -222,17 +237,17 @@ 0 0 0 - 0 + 0 0 - 0 + 0 0 - 94.2475 + 94.2475 0.0037699 - 0 + 0 0 - 0 + 0 0 - 0 + 0 0 @@ -271,8 +286,8 @@ + filename="gz-sim-hydrodynamics-system" + name="gz::sim::systems::Hydrodynamics"> cylinder3_link 1000 0 @@ -281,18 +296,78 @@ 0 0 0 - 0 + 0 0 - 0 + 0 0 - 94.2475 + 94.2475 0.0037699 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + + 4 0 0 0 0 0 + + 40 + + 0.4 + 0 + 0 + 0.4 + 0 + 0.4 + + + + + + + 0.2 + + + + + + + 0.2 + + + + + + + sphere_current_link + 918 + 15.381 + 15.381 + 15.381 + 0 + 0 + 0 + 94.2475 + 0 + 94.2475 + 0 + 94.2475 + 0 0 0 0 0 0 0 + current_x + current_y + current_z diff --git a/test/worlds/hydrodynamics_flags.sdf b/test/worlds/hydrodynamics_flags.sdf index b5cbd2a6cf..3b8d5a0478 100644 --- a/test/worlds/hydrodynamics_flags.sdf +++ b/test/worlds/hydrodynamics_flags.sdf @@ -107,17 +107,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 true true @@ -222,17 +222,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 true @@ -336,17 +336,17 @@ 0 -33.46 -33.46 - -6.2282 + -6.2282 0 - -601.27 + -601.27 0 - -601.27 + -601.27 0 - -0.1916 + -0.1916 0 - -632.698957 + -632.698957 0 - -632.698957 + -632.698957 0 true diff --git a/test/worlds/joint_controller.sdf b/test/worlds/joint_controller.sdf index 1b1d40b375..21675dc327 100644 --- a/test/worlds/joint_controller.sdf +++ b/test/worlds/joint_controller.sdf @@ -117,6 +117,123 @@ + + 0 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.5 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + 0.0 -0.5 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor1 + + 0 0 1 + + + + 0 0 -0.5 0 0 0 + base_link + rotor2 + + 0 0 1 + + + + 5.0 + joints + j1 + j2 + + + 3 3 0.005 0 0 0 @@ -149,7 +266,7 @@ - + 0.0 0.0 1.0 0.0 0 0 0.0 0.0 0.0 0 0 0 @@ -182,7 +299,7 @@ 0 0 -0.5 0 0 0 base_link - rotor2 + rotor3 0 0 1 @@ -196,5 +313,84 @@ 0.01 + + + -3 -3 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.5 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor4 + + 0 0 1 + + + + j4 + true + 0 + + diff --git a/test/worlds/joint_position_controller_actuators.sdf b/test/worlds/joint_position_controller_actuators.sdf new file mode 100644 index 0000000000..5818a79ec7 --- /dev/null +++ b/test/worlds/joint_position_controller_actuators.sdf @@ -0,0 +1,123 @@ + + + + + 0 + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.0 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor + + 0 0 1 + + + + j1 + true + 0 + + + + diff --git a/test/worlds/joint_position_controller_multiple_joints_subtopic.sdf b/test/worlds/joint_position_controller_multiple_joints_subtopic.sdf new file mode 100644 index 0000000000..2e530937a0 --- /dev/null +++ b/test/worlds/joint_position_controller_multiple_joints_subtopic.sdf @@ -0,0 +1,166 @@ + + + + + 0 + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 0 0 0.005 0 0 0 + + 0.0 0.0 0.0 0 0 0 + + + 2.501 + 0 + 0 + 2.501 + 0 + 5 + + 120.0 + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + 0.0 0.0 0.0 0 0 0 + + + 0.5 0.5 0.01 + + + + + + 0.0 0.5 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + 0.0 -0.5 1.0 0.0 0 0 + + 0.0 0.0 0.0 0 0 0 + + 0.032 + 0 + 0 + 0.032 + 0 + 0.00012 + + 0.6 + + + + + 0.25 0.25 0.05 + + + + + + + 0.25 0.25 0.05 + + + + + + + 0 0 -0.5 0 0 0 + base_link + rotor1 + + 0 0 1 + + + + 0 0 -0.5 0 0 0 + base_link + rotor2 + + 0 0 1 + + + + j1 + j2 + joints + 1 + 0.1 + 0.01 + 1 + -1 + 1000 + -1000 + + + + diff --git a/test/worlds/joint_position_controller_nested_models.sdf b/test/worlds/joint_position_controller_nested_models.sdf new file mode 100644 index 0000000000..0a95a1d723 --- /dev/null +++ b/test/worlds/joint_position_controller_nested_models.sdf @@ -0,0 +1,270 @@ + + + + + + + 0 + + + + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 -2 0.5 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 1 1 0 + 1 1 0 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + 1 0 0 0 0 0 + + + 1.0 + + 0.166666667 + 0.00 + 0.00 + 0.166666667 + 0.00 + 0.166666667 + + + + + + 1 1 1 + + + + 0 1 1 + 0 1 1 + 0.1 0.1 0 1 + + + + + + 1 1 1 + + + + + + 0 0 0.6 0 0 0 + + 0.01 + + 1.66667E-05 + 0.00 + 0.00 + 0.000841667 + 0.00 + 0.000841667 + + + + + + 1 0.1 0.1 + + + + 1 0.5 0 + 1 0.5 0 + 0.1 0.1 0 1 + + + + + + 1 0.1 0.1 + + + + + + base_link + rotor_link + + + 0.5 + + + -3.14159265 + 3.14159265 + + 0 0 1 + + + + + base_link + model21::base_link + + + 1 + + + 0 + 0 + + 0 0 1 + + + + rotor_joint + /model2/cmd_rotor + 30 + 0.05 + + + model21::rotor_joint + /model21/cmd_rotor + 30 + 0.05 + + + + + diff --git a/test/worlds/joints_in_world.sdf b/test/worlds/joints_in_world.sdf new file mode 100644 index 0000000000..d838bbecb4 --- /dev/null +++ b/test/worlds/joints_in_world.sdf @@ -0,0 +1,44 @@ + + + + + world + m1 + + + + + + + + 0.5 + + + + + + + + + m1 + m2 + + 0 1 0 + + + + + 2 0 0 0 0 0 + + + + + 0.5 + + + + + + + + diff --git a/test/worlds/lights_render.sdf b/test/worlds/lights_render.sdf index c8ea520c75..bec495ceec 100644 --- a/test/worlds/lights_render.sdf +++ b/test/worlds/lights_render.sdf @@ -16,7 +16,7 @@ - ogre + ogre2 true diff --git a/test/worlds/non_link_components.sdf b/test/worlds/non_link_components.sdf new file mode 100644 index 0000000000..596213031a --- /dev/null +++ b/test/worlds/non_link_components.sdf @@ -0,0 +1,83 @@ + + + + 0 0 -10 + + 0.001 + 1 + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 2 0 0 0 + + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + 0.1 0.2 0 0 1.570796327 0 + + + 0.01 0.01 0.01 + + + + + + + 0.1 0.1 0.1 + + + + + + + + diff --git a/test/worlds/odometry_publisher_3d.sdf b/test/worlds/odometry_publisher_3d.sdf index 49df3a2f66..7b5695e6c0 100644 --- a/test/worlds/odometry_publisher_3d.sdf +++ b/test/worlds/odometry_publisher_3d.sdf @@ -273,7 +273,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -293,7 +293,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -313,7 +313,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -333,7 +333,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/test/worlds/projector.sdf b/test/worlds/projector.sdf new file mode 100644 index 0000000000..bf5facfd83 --- /dev/null +++ b/test/worlds/projector.sdf @@ -0,0 +1,79 @@ + + + + + + 0.001 + 0 + + + + + + + true + 0 0 10 0 0 0 + 1 1 1 1 + 0.5 0.5 0.5 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + + 0 0 0 0 0 0 + true + + + + + 1 1 1 + + + + + 0 1 0 0 0 0 + 2 + 7 + 0.5 + path/to/dummy_image.png + 0x01 + + + + + + diff --git a/test/worlds/quadcopter.sdf b/test/worlds/quadcopter.sdf index 1cd5b22364..bc93489ee5 100644 --- a/test/worlds/quadcopter.sdf +++ b/test/worlds/quadcopter.sdf @@ -262,7 +262,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -282,7 +282,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -302,7 +302,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -322,7 +322,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/test/worlds/quadcopter_velocity_control.sdf b/test/worlds/quadcopter_velocity_control.sdf index 697c5e9072..09390c0274 100644 --- a/test/worlds/quadcopter_velocity_control.sdf +++ b/test/worlds/quadcopter_velocity_control.sdf @@ -262,7 +262,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 0 + 0 8.06428e-05 1e-06 motor_speed/0 @@ -282,7 +282,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 1 + 1 8.06428e-05 1e-06 motor_speed/1 @@ -302,7 +302,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 2 + 2 8.06428e-05 1e-06 motor_speed/2 @@ -322,7 +322,7 @@ 8.54858e-06 0.016 gazebo/command/motor_speed - 3 + 3 8.06428e-05 1e-06 motor_speed/3 diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index ed5e1eaab5..d025b942f9 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -2,7 +2,8 @@ - 0 + 0.001 + 1.0 true + 50 1.047 @@ -103,6 +105,7 @@ " + 40 @@ -129,6 +132,7 @@ + 25 1.05 @@ -152,6 +156,7 @@ + 10 1.05 @@ -168,6 +173,7 @@ + 40 1.15 @@ -184,6 +190,7 @@ + 25 1.0 diff --git a/test/worlds/sensors_system_mem_leak.sdf b/test/worlds/sensors_system_mem_leak.sdf new file mode 100644 index 0000000000..8e144eaa9b --- /dev/null +++ b/test/worlds/sensors_system_mem_leak.sdf @@ -0,0 +1,69 @@ + + + + + + ogre2 + + + + 0 0 0 0 0 0 + true + + + + 5 5 2.5 + + + + + 5 5 2.5 + + + 0 1 0 0.8 + 0 1 0 0.8 + 1 1 1 0.8 + + + + + + + true + -6 2 2 0 0.5 0 + + + + + 0.1 0.1 0.1 + + + + + 1 0 1.3 0 0 0 + + 1.047 + + 320 + 240 + + + 0.1 + 100 + + + gaussian_quantized + 0.01 + 0.0002 + + + 1 + 30 + camera + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index 9be1fe6bc2..451e2c35ea 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -42,14 +42,19 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage spherical_coordinates "Spherical coordinates": Working with latitude and longitude * \subpage python_interfaces Python interfaces * \subpage headless_rendering "Headless rendering": Access the GPU on a remote machine to produce sensor data without an X server. +* \subpage reset_simulation Reset simulation +* \subpage move_camera_to_model Move camera to model **Migration from Gazebo classic** * \subpage migrationplugins "Plugins": Walk through the differences between writing plugins for Gazebo classic and Gazebo * \subpage migrationsdf "SDF": Migrating SDF files from Gazebo classic to Gazebo -* \subpage migrationworldapi "World API": Guide on what World C++ functions to call in Gazebo Sim when migrating from Gazebo classic -* \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Gazebo Sim when migrating from Gazebo classic -* \subpage migrationlinkapi "Link API": Guide on what Link C++ functions to call in Gazebo Sim when migrating from Gazebo classic +* \subpage migrationworldapi "World API": Guide on what World C++ functions to call in Gazebo when migrating from Gazebo classic +* \subpage migrationmodelapi "Model API": Guide on what Model C++ functions to call in Gazebo when migrating from Gazebo classic +* \subpage migrationlightapi "Light API": Guide on what Light C++ functions to call in Gazebo when migrating from Gazebo classic +* \subpage migrationjointapi "Joint API": Guide on what Joint C++ functions to call in Gazebo when migrating from Gazebo classic +* \subpage migrationactorapi "Actor API": Guide on what Actor C++ functions to call in Gazebo when migrating from Gazebo classic +* \subpage migrationlinkapi "Link API": Guide on what Link C++ functions to call in Gazebo when migrating from Gazebo classic * \subpage ardupilot "Case Study": Migrating the ArduPilot ModelPlugin from Gazebo classic to Gazebo. ## License diff --git a/tutorials/battery.md b/tutorials/battery.md index 57b250e07e..fea877ca5e 100644 --- a/tutorials/battery.md +++ b/tutorials/battery.md @@ -12,8 +12,8 @@ All logic for battery consumption are encapsulated in a plugin. ## A perfect battery An ideal battery has a constant voltage while discharging and no internal -resistance. Here's a minimum example of a perfect battery that can be added to a -model: +resistance. Here's a minimum example of a perfect battery that can be added to +a model: ```{.xml} @@ -39,7 +39,9 @@ model: ... ``` -`` is a consumer-specific parameter. You can set this to a high value to see what happens when the battery drains. All others are properties of the battery. +`` is a consumer-specific parameter. You can set this to a +high value to see what happens when the battery drains. All others are +properties of the battery. Next, you can find a description of the SDF parameters used: @@ -58,8 +60,24 @@ there are some issues affecting batteries in Gazebo Blueprint and Citadel. This parameter fixes the issues. Feel free to omit the parameter if you have legacy code and want to preserve the old behavior. -When setting the ``, `` of the battery and its ``, -keep in mind the following formula: +* ``: Start draining battery from the begining of the +simulation. If this is not set the battery draining can only be started +through the topics set through . + +* ``: Topic(s) that can be used to start +power draining. + +* ``: Topic(s) that can be used to stop power +draining. + +* ``: As reported +[here](https://github.com/gazebosim/gz-sim/issues/225), there are some issues +affecting batteries in Ignition Blueprint and Citadel. This parameter fixes the +issues. Feel free to omit the parameter if you have legacy code and want to +preserve the old behavior. + +When setting the ``, `` of the battery and its +``, keep in mind the following formula: `battery_runtime` (hours) = `` * `` / `` @@ -68,7 +86,8 @@ keep in mind the following formula: If `` is not set, the battery drains at a faster (100x) rate. In this case, the battery runtime should be calculated as follows: -`battery_runtime` (hours) = `` * `` / (`` * 100) +`battery_runtime` (hours) = `` * +`` / (`` * 100) ## Try a more realistic battery @@ -76,7 +95,8 @@ In this case, the battery runtime should be calculated as follows: If you need to model a more realistic battery, you can use the following advanced SDF parameters: -* ``: Amount of voltage decrease when no charge (V). +* ``: Amount of voltage decrease when +no charge (V). * ``: Initial charge of the battery (Ah). @@ -89,14 +109,14 @@ Please, refer to the battery specification to set the advanced values. ## Charging -A battery can be charged if the SDF parameter `` is set to true. -Here are the relevant SDF parameters related with charging: +A battery can be charged if the SDF parameter `` is set to +true. Here are the relevant SDF parameters related with charging: * ``: As mentioned, it should be `true` to enable recharging. * ``: Hours taken to fully charge the battery. Keep in mind that -this value assumes no battery load while charging. If the battery is under load, -it will take a longer time to recharge. +this value assumes no battery load while charging. If the battery is under +load, it will take a longer time to recharge. * ``: If true, the start/stop signals for recharging the battery will also be available via topics. The regular Gazebo services will @@ -104,8 +124,10 @@ still be available. By default, two Gazebo Transport services are available for managing charging: -* `/model//battery//recharge/start`: Enable recharging. -* `/model//battery//recharge/stop`: Disable recharging. +* `/model//battery//recharge/start`: +Enable recharging. +* `/model//battery//recharge/stop`: +Disable recharging. Both services accept an `gz::msgs::Boolean` parameter. @@ -117,8 +139,9 @@ A battery has been added to a demo world, which can be run using: gz sim -v 4 linear_battery_demo.sdf -z 1000000 ``` -The blue vehicle on the left has a battery, while the one on the right does not. When the battery drains, the corresponding vehicle stops moving. Please, see -`gz-sim/examples/worlds/linear_battery_demo.sdf`, where you can +The blue vehicle on the left has a battery, while the one on the right does +not. When the battery drains, the corresponding vehicle stops moving. Please, +see `gz-sim/examples/worlds/linear_battery_demo.sdf`, where you can find the commands to visualize the state of the battery, as well as commands to start and stop the recharging. @@ -131,9 +154,145 @@ mkdir build && cd build cmake .. && make ./keyboard ../keyboard.sdf ``` -See more about the usage of the keyboard plugin in `examples/standalone/keyboard/README.md`. +See more about the usage of the keyboard plugin in +`examples/standalone/keyboard/README.md`. + +## Creating battery consumers from other systems + +You can easily create battery consumers from other systems by creating a +consumer entity that holds a `BatteryPowerLoad` component. + +### Example of the Thruster system as a battery consumer + +It is entirely up to the developer to set the means and ways to calculate +the power load of the system and the options for future users of the system. +Basically all the system needs to do is to set up an entity and add the +`BatteryPowerLoad` component with the corresponding information, the rest is +up to the developer. However the `Thruster` system is a good example of how to +make a system consume power from a certain battery. + +The way the `Thruster` system allows users to set it as a battery consumer is +through the options `` and ``. Through these options +the system user can set how much power load should the system consume and which +battery should it use. Take note that, with this approach of identifying +batteries the user would have to ensure that the battery names are unique +within the system. + +#### Configure step + +At the configure step the parameters are read and saved: + +``` + if (_sdf->HasElement("power_load")) + { + if (!_sdf->HasElement("battery_name")) + { + ignerr << "Specified a but missing ." + "Specify a battery name so the power load can be assigned to it." + << std::endl; + } + else + { + this->dataPtr->powerLoad = _sdf->Get("power_load"); + this->dataPtr->batteryName = _sdf->Get("battery_name"); + } + } +``` + +Note that both are required, if no `power_load` present then `battery_name` is +ignored and if no `battery_name` is there when a `power_load` is set an error +will be thrown asking for this parameter. + +#### PreUpdate step + +During this step and only for one time the battery consumer will be set. This +is done in this step instead of doing it in the configure step to avoid race +conditions and make sure all the batteries are ready. The `batteryInitialized` +makes sure the initialization only happens once. + +``` + // Intit battery consumption if it was set + if (!this->dataPtr->batteryName.empty() && + !this->dataPtr->batteryInitialized) + { + this->dataPtr->batteryInitialized = true; + ... +``` + +The battery entity is searched using the `ecm` and saved +to later add the info to the `BatteryPowerLoad` component. + +``` + // Check that a battery exists with the specified name + Entity batteryEntity; + int numBatteriesWithName = 0; + _ecm.Each( + [&](const Entity &_entity, + const components::BatterySoC */*_BatterySoC*/, + const components::Name *_name)->bool + { + if (this->dataPtr->batteryName == _name->Data()) + { + ++numBatteriesWithName; + batteryEntity = _entity; + } + return true; + }); +``` + +Some errors are thrown if no batteries or more than one batteries +are found with the specified name. + +``` + if (numBatteriesWithName == 0) + { + ignerr << "Can't assign battery consumption to battery: [" + << this->dataPtr->batteryName << "]. No batteries" + "were found with the given name." << std::endl; + return; + } + if (numBatteriesWithName > 1) + { + ignerr << "More than one battery found with name: [" + << this->dataPtr->batteryName << "]. Please make" + "sure battery names are unique within the system." + << std::endl; + return; + } +``` + +Note that in this example the name is used to uniquely identify +a battery within a system but it is up to the developer to use any +other means they consider adequate. I.e. some sort of hierarchy can +be enforced in the system to help identify batteries using this hierarchy. + +Finally, the consumer entity is created and the power load and battery +info added to its `BatteryPowerLoad` component: + +``` + // Create the battery consumer entity and its component + this->dataPtr->consumerEntity = _ecm.CreateEntity(); + components::BatteryPowerLoadInfo batteryPowerLoadInfo{ + batteryEntity, this->dataPtr->powerLoad}; + _ecm.CreateComponent(this->dataPtr->consumerEntity, + components::BatteryPowerLoad(batteryPowerLoadInfo)); + _ecm.SetParentEntity(this->dataPtr->consumerEntity, batteryEntity); + } +``` + +The `consumerEntity` is saved in case it has to be modified or even +deleted in the future. If a developer wants to change the power load they +just need to modify the component, setting it to 0 will stop the consumer +effect on the battery. Another option is to entirely delete the consumer +entity. +The battery plugin basically checks for entities with a `BatteryPowerLoad` +component and uses that information to calculate the total battery consumption +before the draining step happens. By modifying the values and quantities of +entities with `BatteryPowerLoad` components developers can add, modify and +remove battery consumers from the different batteries available in a system. ## Known Issues -* The rate of consumption should be affected by torque. For example, going uphill should consume more power than going downhill. +* The rate of consumption should be affected by torque. For example, going +uphill should consume more power than going downhill. diff --git a/tutorials/blender_procedural_datasets.md b/tutorials/blender_procedural_datasets.md index e98f8a03be..c0a27f4c84 100644 --- a/tutorials/blender_procedural_datasets.md +++ b/tutorials/blender_procedural_datasets.md @@ -93,7 +93,7 @@ blender [blender options] file.blend 4. Run the script using the *Run script* button in the panel of the *Text Editor* tab at the top of the screen. -@image html https://github.com/gazebosim/gz-sim/tree/main/tutorials/files/blender_procedural_datasets/blender_instructions.png "Instructions in Blender" width=100% +![Instructions in Blender](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/blender_procedural_datasets/blender_instructions.png) Once you follow these steps and configure the script for your `.blend` project, you can save it and use Option B in the future. @@ -155,9 +155,9 @@ blender rock.blend --python-text procedural_dataset_generator.py -- -o sdf_model blender woodland.blend --python-text procedural_dataset_generator.py -- -o sdf_models/woodland ``` -@image html https://github.com/gazebosim/gz-sim/tree/main/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif "Example of generating a dataset of rock SDF models" width=100% +![Example of generating a dataset of rock SDF models](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/blender_procedural_datasets/demo_blender_rock.gif) -@image html https://github.com/gazebosim/gz-sim/tree/main/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif "Example of generating a dataset of woodland SDF models" width=100% +![Example of generating a dataset of woodland SDF models](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/blender_procedural_datasets/demo_blender_woodland.gif) You can configure the script in several ways (see `blender rock.blend --python-text procedural_dataset_generator.py -- -h`). For @@ -220,9 +220,9 @@ Hereafter, you can spawn the generated models inside Gazebo with your preferred approach, e.g. via the Resource Spawner GUI plugin. Below are some examples of Gazebo environments using the rock and woodland SDF models. -@image html https://github.com/gazebosim/gz-sim/tree/main/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png "Example of the generated rock SDF models in Gazebo" width=100% +![Example of the generated rock SDF models in Gazebo](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/blender_procedural_datasets/demo_gazebo_rock.png) -@image html https://github.com/gazebosim/gz-sim/tree/main/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png "Example of the generated woodland SDF models in Gazebo" width=100% +![Example of the generated woodland SDF models in Gazebo](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/blender_procedural_datasets/demo_gazebo_woodland.png) Every object that uses Geometry Nodes in these projects has several input attributes that can be configured. You can open the `.blend` projects again, diff --git a/tutorials/create_system_plugins.md b/tutorials/create_system_plugins.md index cc9bafabe0..dec24591d0 100644 --- a/tutorials/create_system_plugins.md +++ b/tutorials/create_system_plugins.md @@ -15,8 +15,8 @@ steps below. ## Decide on interfaces to implement The first step of implementing a system plugin is to determine the subset of -available interfaces to implement. Aside from the base `System` object, -there are currently three additional available interfaces: +available interfaces to implement. Aside from the base `System` object, +there are currently four additional available interfaces: 1. ISystemConfigure 1. Has read-write access to world entities and components. @@ -27,13 +27,16 @@ there are currently three additional available interfaces: 1. Has read-write access to world entities and components. 2. This is where systems say what they'd like to happen at time gz::sim::UpdateInfo::simTime. 3. Can be used to modify state before physics runs, for example for applying control signals or performing network synchronization. -2. ISystemUpdate +3. ISystemUpdate 1. Has read-write access to world entities and components. 2. Used for physics simulation step (i.e., simulates what happens at time gz::sim::UpdateInfo::simTime). -3. ISystemPostUpdate +4. ISystemPostUpdate 1. Has read-only access to world entities and components. 2. Captures everything that happened at time gz::sim::UpdateInfo::simTime. 3. Used to read out results at the end of a simulation step to be used for sensor or controller updates. +5. ISystemReset + 1. Has read-write access to world entities and components. + 2. Executed once the moment the plugin is reseted. It's important to note that gz::sim::UpdateInfo::simTime does not refer to the current time, but the time reached after the `PreUpdate` and `Update` calls have finished. So, if any of the `*Update` functions are called with simulation paused, time does not advance, which means the time reached after `PreUpdate` and `Update` is the same as the starting time. diff --git a/tutorials/files/move_camera_to_model/box_20.png b/tutorials/files/move_camera_to_model/box_20.png new file mode 100644 index 0000000000..64dd32cf47 Binary files /dev/null and b/tutorials/files/move_camera_to_model/box_20.png differ diff --git a/tutorials/files/move_camera_to_model/box_5.png b/tutorials/files/move_camera_to_model/box_5.png new file mode 100644 index 0000000000..e6841357ee Binary files /dev/null and b/tutorials/files/move_camera_to_model/box_5.png differ diff --git a/tutorials/files/move_camera_to_model/move_to_model.gif b/tutorials/files/move_camera_to_model/move_to_model.gif new file mode 100644 index 0000000000..6518d805c4 Binary files /dev/null and b/tutorials/files/move_camera_to_model/move_to_model.gif differ diff --git a/tutorials/files/reset_simulation/reset_button.gif b/tutorials/files/reset_simulation/reset_button.gif new file mode 100644 index 0000000000..205e97afec Binary files /dev/null and b/tutorials/files/reset_simulation/reset_button.gif differ diff --git a/tutorials/migrating_ardupilot_plugin.md b/tutorials/migrating_ardupilot_plugin.md index ff39e0009a..9a31e6a19a 100644 --- a/tutorials/migrating_ardupilot_plugin.md +++ b/tutorials/migrating_ardupilot_plugin.md @@ -15,7 +15,7 @@ can be found in [this fork](https://github.com/gerkey/ardupilot_gazebo/tree/igni ## Background The `ardupilot_gazebo` plugin is used with Gazebo to assist with simulating -unmanned aerial vehicles (UAVs, aka drones). For more information on how to use +aerial vehicles (aka drones). For more information on how to use it, check the [ArduPilot documentation](https://ardupilot.org/dev/docs/using-gazebo-simulator-with-sitl.html). diff --git a/tutorials/migration_actor_api.md b/tutorials/migration_actor_api.md new file mode 100644 index 0000000000..d1aacda04f --- /dev/null +++ b/tutorials/migration_actor_api.md @@ -0,0 +1,247 @@ +\page migrationactorapi + +# Migration from Gazebo-classic: Actor API + +When migrating plugins from Gazebo-classic to Gazebo, developers will +notice that the C++ APIs for both simulators are quite different. Be sure to +check the [plugin migration tutorial](migrationplugins.html) to get a high-level +view of the architecture differences before using this guide. + +This tutorial is meant to serve as a reference guide for developers migrating +functions from the +[gazebo::phyiscs::Actor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Actor.html) +class. + +If you're trying to use some API which doesn't have an equivalent on Gazebo +yet, feel free to +[ticket an issue](https://github.com/gazebosim/gz-sim/issues/). + +## Actor API + +Gazebo-classic's `gazebo::physics::Actor` provides lots of functionality, which +can be divided in these categories: + +* **Properties**: Setting / getting properties + * Example: [Actor::GetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#a9a98946a64f3893b085f650932c9dfee) / [Actor::SetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a5d74ac4d7a230aed1ab4b11933b16e92) +* **Read family**: Getting children and parent + * Example: [Actor::GetParentModel](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a94d6f01102b5d949006fba3628d9f355) +* **Write family**: Adding children, changing parent + * Example: [Actor::RemoveChildren](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#aa85d2386e6fb02bdbec060a74b63238a) +* **Lifecycle**: Functions to control the actor's lifecycle + * Example: [Actor::Init](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Actor.html#ae048ef824aaf614707c1496a2aefd415) +* **Others**: Functions that don't fit any of the categories above + * Example: [Actor::PlaceOnEntity](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a9ecbfeb56940cacd75f55bed6aa9fcb4) + +You'll find the Gazebo APIs below on the following headers: + +* [ignition/gazebo/Actor.hh](https://gazebosim.org/api/gazebo/7/Actor_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) + +It's worth remembering that most of this functionality can be performed using +the +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) +directly. + + +As an example the `Actor::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. + +``` + math::Pose3d pose = _ecm.Component(actorEntityId)->Data(); +``` + +The functions presented in the sections below exist for convenience and +readability. The items marked as `TODO` means that the equivalent API is not +implemented yet in Gazebo. + +### Properties + +Most of Gazebo-classic's Actor API is related to setting and getting +properties that are done via the Entity-Component-System by setting components +(properties) into entities such as actors. Note that a number of the public APIs +in Classic are inherited from the base Model / Entity class and they may not +have equivalents in Gazebo. + +--- + +Classic | Gazebo +-- | -- +AddType | `ecm.CreateComponent(entity, Type())` +AlignBvh | TODO +BoundingBox | TODO +CollisionBoundingBox | TODO +CustomTrajectory | see `SetCustomTrajectory` +DirtyPose | Not supported +FillMsg | TODO +GetAutoDisable | TODO +GetId | `ignition::gazebo::Model::Entity` +GetName | `ignition::gazebo::Actor::Name` +GetPluginCount | TODO +GetSaveable | Not supported +GetScopedName | `ignition::gazebo::scopedName` +GetSDF | TODO +GetSDFDom | TODO +GetSelfCollide | TODO +GetType | `ignition::gazebo::entityType` +GetWorldEnergy | TODO +GetWorldEnergyKinetic | TODO +GetWorldEnergyPotential | TODO +HasType | `gazebo::components::Link::typeId == entityTypeId(entity, ecm)` +InitialRelativePose | TODO +IsActive | TODO +IsCanonicalLink | Not applicable. See link API +IsSelected | Selection is client-specific, not porting +IsStatic | TODO +Mesh | TODO +Play | TODO +PluginInfo | TODO +Print | TODO +ProcessMsg | TODO +RelativeAngularAccel | TODO +RelativeAngularVel | TODO +RelativeLinearAccel | TODO +RelativeLinearVel | TODO +RelativePose | `ignition::gazebo::Actor::Pose` +Scale | TODO +ScriptTime | `ignition::gazebo::Actor::AnimationTime` +SDFPoseRelativeToParent | `ignition::gazebo::Actor::Pose` +SDFSemanticPose | `ignition::gazebo::Actor::Pose` +SensorScopedName | TODO +SetAngularVel | TODO +SetAnimation | use `ignition::gazebo::Actor::SetTrajectoryPose` +SetAutoDisable | TODO +SetCollideMode | TODO +SetCustomTrajectory | use `ignition::gazebo::Actor::SetTrajectoryPose`, `SetAnimationTime`, and `SetAnimationName` to achieve similar result. +SetEnabled | TODO +SetGravityMode | TODO +SetInitialRelativePose | TODO +SetJointAnimation | Not supported +SetJointPosition | Not supported +SetJointPositions | Not supported +SetLaserRetro | TODO +SetLinearVel | TODO +SetLinkWorldPose | TODO +SetName | TODO +SetRelativePose | TODO +SetSaveable | Not supported +SetScale | TODO +SetScriptTime | `ignition::gazebo::Actor::SetAnimationTime` +SetSelected | Selection is client-specific, not porting +SetSelfCollide | TODO +SetState | TODO +SetStatic | TODO +SetWindMode | TODO +SetWorldPose | TODO +SetWorldTwist | TODO +SkeletonAnimations | TODO +Stop | TODO +StopAnimation | use `ignition::gazebo::Actor::SetTrajectoryPose` +TypeStr | `ignition::gazebo::entityTypeStr` +UnscaledSDF | TODO +UpdateParamenters | TODO +URI | TODO +WindMode | TODO +WorldAngularAccel | TODO +WorldAngularVel | TODO +WorldLinearAccel | TODO +WorldLinearVel | TODO +WorldPose | `ignition::gazebo::Actor::WorldPose` + +--- + +## Read family + +These APIs deal with reading information related to child / parent +relationships. + +The main difference in these APIs across Gazebo generations is that +on classic, they deal with shared pointers to entities, while on Gazebo, +they deal with entity IDs. + +--- + +Classic | Gazebo +-- | -- +GetByName | TODO +GetChild | TODO +GetChildCollision | TODO +GetChildCount | TODO +GetChildLink | TODO +GetGripper | Not supported +GetGripperCount | Not supported +GetJoint | Not supported +GetJointCount | Not supported +GetJoints | Not supported +GetLink | TODO +GetLinks | TODO +GetParent | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetParentId | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetParentModel | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetSensorCount | TODO +GetWorld | const `ignition::gazebo::worldEntity` +NestedModel | TODO +NestedModels | TODO + +--- + +## Write family + +These functions deal with modifying the entity tree, attaching children to new +parents. Note that APIs for changing an Actor's entity tree structure are +currently not implemented yet. + +--- + +Classic | Gazebo +-- | -- +AddChild | TODO +AttachStaticModel | TODO +DetachStaticModel | TODO +RemoveChild | TODO +RemoveChildren | TODO +RemoveJoint | TODO +SetCanonicalLink | TODO +SetParent | TODO +SetWorld | TODO + +--- + +## Lifecycle + +These functions aren't related to the state of a actor, but perform some +processing related to the actor's lifecycle, like initializing, updating or +terminating it. + +--- + +Classic | Gazebo +-- | -- +Fini | N/A +Init | N/A +Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadJoints | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadPlugins | TODO +Reset | TODO +ResetCustomTrajectory | TODO +ResetPhysicsStates | TODO +Update | Entities are updated by systems + +--- + + +## Others + +Miscelaneous functions that don't fit the other categories. Most of them involve +logic that should be performed from within a system. + +--- + +Classic | Gazebo +-- | -- +GetJointController | Use this system: `ignition::gazebo::systems::JointController` +GetNearestEntityBelow | Requires a system +PlaceOnEntity | Requires a system +PlaceOnNearestEntityBelow | Requires a system + +--- diff --git a/tutorials/migration_joint_api.md b/tutorials/migration_joint_api.md new file mode 100644 index 0000000000..54179c31c3 --- /dev/null +++ b/tutorials/migration_joint_api.md @@ -0,0 +1,203 @@ +\page migrationjointapi + +# Migration from Gazebo-classic: Joint API + +When migrating plugins from Gazebo-classic to Gazebo, developers will +notice that the C++ APIs for both simulators are quite different. Be sure to +check the [plugin migration tutorial](migrationplugins.html) to get a high-level +view of the architecture differences before using this guide. + +This tutorial is meant to serve as a reference guide for developers migrating +functions from the +[gazebo::phyiscs::Joint](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Joint.html) +class. + +If you're trying to use some API which doesn't have an equivalent on Gazebo +yet, feel free to +[ticket an issue](https://github.com/gazebosim/gz-sim/issues/). + +## Joint API + +Gazebo-classic's `gazebo::physics::Joint` provides lots of functionality, which +can be divided in these categories: + +* **Properties**: Setting / getting properties + * Example: [Joint::Anchor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Joint.html#a73e5d867a0cbe1dff76957425a58c9d0) / [Joint::SetAnchor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Joint.html#a9698c1294d75ae76026842a35750afa5) +* **Read family**: Getting children and parent + * Example: [Joint::GetChild](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#a3e8e42332400d79fbeb7f721350ab980) +* **Write family**: Adding children, changing parent + * Example: [Joint::RemoveChildren](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#aa85d2386e6fb02bdbec060a74b63238a) +* **Lifecycle**: Functions to control the joint's lifecycle + * Example: [Joint::Init](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Joint.html#ae048ef824aaf614707c1496a2aefd415) + +You'll find the Gazebo APIs below on the following headers: + +* [ignition/gazebo/Joint.hh](https://gazebosim.org/api/gazebo/7/Joint_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) + +It's worth remembering that most of this functionality can be performed using +the +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) +directly. + +As an example the `Join::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. + +``` + math::Pose3d pose = _ecm.Component(jointEntityId)->Data(); +``` + +The functions presented in the sections below exist for convenience and +readability. The items marked as `TODO` means that the equivalent API is not +implemented yet in Gazebo. + +### Properties + +Most of Gazebo-classic's Joint API is related to setting and getting +properties. These functions are great candidates to have equivalents on +Gazebo, because the Entity-Component-System architecture is perfect for setting +components (properties) into entities such as joints. + +--- + +Classic | Gazebo +-- | -- +AddType | `ecm.CreateComponent(entity, Type())` +Anchor | TODO +AnchorErrorPose | TODO +ApplyStiffnessDamping | TODO +AreConnected | TODO +AxisFrame | TODO +AxisFrameOffset | TODO +CacheForceTorque | TODO +CheckAndTruncateForce | TODO +DOF | TODO +FillMsg | TODO +GetDamping | TODO +GetEffortLimit | TODO +GetForce | `ignition::gazebo::Joint::TransmittedWrench` +GetForceTorque | `ignition::gazebo::Joint::TransmittedWrench` +GetId | `ignition::gazebo::Joint::Entity` +GetInertiaRatio | TODO +GetJointLink | use `ignition::gazebo::Joint::*LinkName` +GetMsgType | TODO +GetName | `ignition::gazebo::Joint::Name` +GetParam | TODO +GetSaveable | Not supported +GetScopedName | `ignition::gazebo::scopedName` +GetSDF | TODO +GetSDFDom | TODO +GetSpringReferencePosition | TODO +GetStiffness | TODO +GetStopDissipation | TODO +GetStopStiffness | TODO +GetType | `ignition::gazebo::Joint::Type` +GetVelocity | `ignition::gazebo::Joint::Velocity` +GetVelocityLimit | TODO +GetWorldEnergyPotentialSpring | TODO +GlobalAxis | TODO +HasType | `gazebo::components::Joint::typeId == entityTypeId(entity, ecm)` +InertiaRatio | TODO +InitialAnchorPose | TODO +IsSelected | Selection is client-specific, not porting +LinkForce | TODO +LinkTorque | TODO +LocalAxis | `ignition::gazebo::Joint::Axis` +LowerLimit | TODO +ParentWorldPose | TODO +Position | `ignition::gazebo::Joint::Position` +Print | TODO +ResolveAxisXyz | TODO +SDFPoseRelativeToParent | `ignition::gazebo::Joint::Pose` +SDFSemanticPose | `ignition::gazebo::Joint::Pose` +SetAnchor | TODO +SetAxis | TODO +SetDamping | TODO +SetEffortLimit | `ignition::gazebo::Joint::SetEffortLimits` +SetForce | `ignition::gazebo::Joint::SetForce` +SetLowerLimit | `ignition::gazebo::Joint::SetPositionLimits` +SetName | TODO +SeParam | TODO +SetPosition | `ignition::gazebo::Joint::ResetPosition` +SetProvideFeedback | `ignition::gazebo::Joint::EnableTransmittedWrenchCheck` +SetSaveable | Not supported +SetSelected | Selection is client-specific, not porting +SetState | TODO +SetStiffness | TODO +SetStiffnessDamping | TODO +SetStopDissipation | TODO +SetStopStiffness | TODO +SetUpperLimit | `ignition::gazebo::Joint::SetPositionLimits` +SetVelocity | `ignition::gazebo::Joint::SetVelocity` +SetVelocityLimit | `ignition::gazebo::Joint::SetVelocityLimits` +TypeStr | `ignition::gazebo::Joint::Type` +UpdateParameters | TODO +UpperLimit | TODO +URI | TODO +WorldPose | TODO + +--- + +## Read family + +These APIs deal with reading information related to child / parent +relationships. + +The main difference in these APIs across Gazebo generations is that +on classic, they deal with shared pointers to entities, while on Gazebo, +they deal with entity IDs. + +--- + +Classic | Gazebo +-- | -- +GetByName | Use type-specific `ignition::gazebo::Joint::*ByName` +GetChild | Use type-specific `ignition::gazebo::Joint::*ByName` +GetChild (Child link) | `ignition::gazebo::Joint::ChildLinkName` +GetChildCount | Use type-specific `ignition::gazebo::Joint::*Count` +GetParent | `ignition::gazebo::Joint::ParentModel` +GetParent (Parent link) | `ignition::gazebo::Joint::ParentLinkName` +GetParentId | `ignition::gazebo::EntiyComponentManager::ParentEntity` +GetWorld | `ignition::gazebo::worldEntity` + +--- + +## Write family + +These functions deal with modifying the entity tree, attaching children to new +parents. Note that APIs for changing a Joint's entity tree structure are +currently not implemented yet. + +--- + +Classic | Gazebo +-- | -- +Attach | TODO +Detach | TODO +RemoveChild | TODO +RemoveChildren | TODO +SetModel | TODO +SetParent | TODO +SetWorld | TODO + +--- + +## Lifecycle + +These functions aren't related to the state of a joint, but perform some +processing related to the joint's lifecycle, like initializing, updating or +terminating it. + +--- + +Classic | Gazebo +-- | -- +ConnectJointUpdate | TODO +Fini | N/A +Init | N/A +Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` +Reset | `ignition::gazebo::Joint::ResetPosition` / `ignition::gazebo::Joint::ResetVelocity` +Update | Entities are updated by systems + +--- diff --git a/tutorials/migration_light_api.md b/tutorials/migration_light_api.md new file mode 100644 index 0000000000..79ea920ddd --- /dev/null +++ b/tutorials/migration_light_api.md @@ -0,0 +1,155 @@ +\page migrationlightapi + +# Migration from Gazebo-classic: Light API + +When migrating plugins from Gazebo-classic to Gazebo, developers will +notice that the C++ APIs for both simulators are quite different. Be sure to +check the [plugin migration tutorial](migrationplugins.html) to get a high-level +view of the architecture differences before using this guide. + +This tutorial is meant to serve as a reference guide for developers migrating +functions from the +[gazebo::rendering::Light](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/rendering_1_1Light.html) +class. + +If you're trying to use some API which doesn't have an equivalent on Gazebo +yet, feel free to +[ticket an issue](https://github.com/gazebosim/gz-sim/issues/). + +## Light API + +Gazebo-classic has 2 Light classes: +* [`gazebo::rendering::Light`](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1rendering_1_1Light.html) - responsible for accessing and setting light properties. + * Example: [Light::DiffuseColor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1rendering_1_1Light.html#a0deb81873bee2c7bc883a10c373501d0) / [Light::SetDiffuseColor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1rendering_1_1Light.html#a9208ba6d4cb8e0972e046e735dc26976) +* [`gazebo::physics::Light`](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Light.html) - responsible for accessing and setting generic entity properties, and parent / child relationship. + * Example: [Light::GetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#a9a98946a64f3893b085f650932c9dfee) / [Light::SetName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Entity.html#a5d74ac4d7a230aed1ab4b11933b16e92) + * Example: [Light::GetParent](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#af87578478aeeb2b176de010d1d639fd9) / [Light::SetParent](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1physics_1_1Base.html#a736efe8278da4ecb1640100a2857756f) + +In Gazebo, the light APIs has been consolidated into a single Light class with +some of generic functions available through other utility / core classes. +You'll find the APIs below on the following headers: + +* [ignition/gazebo/Light.hh](https://gazebosim.org/api/gazebo/7/Light_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) + +It's worth remembering that most of this functionality can be performed using +the +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) +directly. + +As an example the `Light::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. + +``` + math::Pose3d pose = _ecm.Component(lightEntityId)->Data(); +``` + +The functions presented in the sections below exist for convenience and +readability. The items marked as `TODO` means that the equivalent API is not +implemented yet in Gazebo. + +### Properties + +Most of Gazebo-classic's Light API is related to setting +and getting properties. These functions are great candidates to have +equivalents on Gazebo because the Entity-Component-System architecture is +perfect for setting components (properties) into entities such as lights. +This section focuses on migrating from APIs provided through the +`gazebo::rendering::Light` class. + +--- + +Classic | Gazebo +-- | -- +CastShadows | `ignition::gazebo::Light::CastShadows` +Clone | TODO +DiffuseColor | `ignition::gazebo::Light::DiffuseColor` +Direction | `ignition::gazebo::Light::Direction` +FillMsg | TODO +Id | `ignition::gazebo::Light::Entity` +LightType | `ignition::gazebo::Light::Type` +Name | `ignition::gazebo::Light::Name` +Position | `ignition::gazebo::Light::Pose` +Rotation | `ignition::gazebo::Light::Pose` +SetAttenuation | use `ignition::gazebo::Light::SetAttenuation*` +SetCastShadows | `ignition::gazebo::Light::SetCastShadows` +SetDiffuseColor | `ignition::gazebo::Light::SetDiffuseColor` +SetDirection | `ignition::gazebo::Light::SetDirection` +SetLightType | TODO +SetName | TODO +SetPosition | TODO +SetRange | `ignition::gazebo::Light::SetAttenuationRange` +SetRotation | TODO +SetSelected | Selection is client-specific, not porting +SetSpecularColor | `ignition::gazebo::Light::SetSpecularColor` +SetSpotFalloff | `ignition::gazebo::Light::SetSpotFalloff` +SetSpotInnerAngle | `ignition::gazebo::Light::SetSpotInnerAngle` +SetSpotOuterAngle | `ignition::gazebo::Light::SetSpotOuterAngle` +SetVisible | TODO +ShowVisual | TODO +SpecularColor | `ignition::gazebo::Light::SetSpecularColor` +ToggleShowVisual | TODO +Type | `ignition::gazebo::Light::Type` +Visible | TODO +WorldPose | TODO +--- + + +## Read family + +These APIs deal with reading information related to child / parent +relationships. As mentioned earlier, these APIs in Gazebo +classic are provided by the `gazebo::physics::Light` class. Most of the +APIs are inherited from the base `gazebo::physics::Entity` class in classic. +We will only list the relevant ones here. + +The main difference in these APIs across Gazebo generations is that +on classic, they deal with shared pointers to entities, while on Gazebo, +they deal with entity IDs. + +--- + +Classic | Gazebo +-- | -- +GetParent | `ignition::gazebo::Light::Parent` +GetParentId | `ignition::gazebo::Light::Parent` +GetWorld | `ignition::gazebo::worldEntity` + +--- + +## Write family + +These functions deal with modifying the entity tree, attaching children to new +parents. As mentioned earlier, these APIs in Gazebo classic are provided by the +`gazebo::physics::Light` class. Most of the APIs are inherited from the base +`gazebo::physics::Entity` class in classic. We will only list the relevant ones +here. As seen below, APIs for changing a Light's entity tree structure are +currently not implemented yet. + +--- + +Classic | Gazebo +-- | -- +AddChild | Not supported +RemoveChild | Not supported +RemoveChildren | Not supported +SetParent | TODO +SetWorld | TODO + +--- + +## Lifecycle + +These functions are related to the light's lifecycle, like loading and updating +its properties. + +--- + +Classic | Gazebo +-- | -- +Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` +LoadFromMsg | `ignition::gazebo::SdfEntityCreator::CreateEntities` +UpdateFromMsg | TODO + +--- diff --git a/tutorials/migration_link_api.md b/tutorials/migration_link_api.md index 7a3545a5e7..ae7890a984 100644 --- a/tutorials/migration_link_api.md +++ b/tutorials/migration_link_api.md @@ -261,7 +261,7 @@ logic that should be performed from within a system. Classic | Gazebo -- | -- GetNearestEntityBelow | Requires a system -PlaceOnEntity | Involves Requires a system +PlaceOnEntity | Requires a system PlaceOnNearestEntityBelow | Requires a system --- diff --git a/tutorials/migration_model_api.md b/tutorials/migration_model_api.md index ade59b3921..837ac603d5 100644 --- a/tutorials/migration_model_api.md +++ b/tutorials/migration_model_api.md @@ -218,7 +218,7 @@ Classic | Gazebo -- | -- GetJointController | Use this system: `gz::sim::systems::JointController` GetNearestEntityBelow | Requires a system -PlaceOnEntity | Involves Requires a system +PlaceOnEntity | Requires a system PlaceOnNearestEntityBelow | Requires a system --- diff --git a/tutorials/migration_sdf.md b/tutorials/migration_sdf.md index a4abb6c977..daece419ab 100644 --- a/tutorials/migration_sdf.md +++ b/tutorials/migration_sdf.md @@ -43,12 +43,12 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Sun + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Sun - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Ground Plane + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Ground Plane @@ -59,7 +59,7 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae @@ -67,7 +67,7 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Radio/4/files/meshes/Radio.dae @@ -76,11 +76,11 @@ For example, this world can be loaded into both simulators: - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae 1.0 - https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae + https://fuel.gazebosim.org/1.0/OpenRobotics/models/actor - relative paths/tip/files/meshes/talk_b.dae 1.0