diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a415239685..97824d19bf 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,16 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'gz-sim7' + +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. +concurrency: + group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} + cancel-in-progress: true jobs: focal-ci: diff --git a/.github/workflows/triage.yml b/.github/workflows/triage.yml index 6f93ccd58f..2332244bf4 100644 --- a/.github/workflows/triage.yml +++ b/.github/workflows/triage.yml @@ -10,9 +10,7 @@ jobs: runs-on: ubuntu-latest steps: - name: Add ticket to inbox - uses: technote-space/create-project-card-action@v1 + uses: actions/add-to-project@v0.5.0 with: - PROJECT: Core development - COLUMN: Inbox - GITHUB_TOKEN: ${{ secrets.TRIAGE_TOKEN }} - CHECK_ORG_PROJECT: true + project-url: https://github.com/orgs/gazebosim/projects/7 + github-token: ${{ secrets.TRIAGE_TOKEN }} diff --git a/CMakeLists.txt b/CMakeLists.txt index 5644db73f7..988eb27539 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,12 +38,19 @@ else() set (EXTRA_TEST_LIB_DEPS) endif() -include(test/find_dri.cmake) -FindDRI() +# We're disabling pybind11 by default on Windows because they +# don't have active CI on them for now. +set(skip_pybind11_default_value OFF) +if (MSVC) + set(skip_pybind11_default_value ON) +endif() option(SKIP_PYBIND11 "Skip generating Python bindings via pybind11" - OFF) + ${skip_pybind11_default_value}) + +include(test/find_dri.cmake) +FindDRI() include(CMakeDependentOption) cmake_dependent_option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION @@ -236,7 +243,7 @@ add_subdirectory(examples) #============================================================================ gz_create_packages() -if (${pybind11_FOUND}) +if (pybind11_FOUND) add_subdirectory(python) endif() #============================================================================ diff --git a/Changelog.md b/Changelog.md index 3097aec077..56f17f3edf 100644 --- a/Changelog.md +++ b/Changelog.md @@ -727,6 +727,173 @@ ## Gazebo Sim 6.x + +### Gazebo Sim 6.15.0 (2023-08-16) + +1. Fix Joint Position Controller Behaviour Described in #1997 + * [Pull request #2001](https://github.com/gazebosim/gz-sim/pull/2001) + +1. Fix a minor issue in the documentation of the server API + * [Pull request #2067](https://github.com/gazebosim/gz-sim/pull/2067) + +1. Use sdf::Element::FindElement instead of GetElement in ApplyLinkWrench + * [Pull request #2052](https://github.com/gazebosim/gz-sim/pull/2052) + +1. Backport sensors system threading optimization changes + * [Pull request #2058](https://github.com/gazebosim/gz-sim/pull/2058) + +1. Adds a warning if the `Server` method of a `TestFixture` is called before `Finalize` + * [Pull request #2047](https://github.com/gazebosim/gz-sim/pull/2047) + +1. Protobuf: Do not require version 3 do support Protobuf 4.23.2 (23.2) + * [Pull request #2006](https://github.com/gazebosim/gz-sim/pull/2006) + +1. Disable pybind11 on Windows by default + * [Pull request #2005](https://github.com/gazebosim/gz-sim/pull/2005) + +1. Print an error message when trying to load SDF files that don't contain a `` + * [Pull request #1998](https://github.com/gazebosim/gz-sim/pull/1998) + +1. Port record topic fix + * [Pull request #2004](https://github.com/gazebosim/gz-sim/pull/2004) + +1. Allow re-attaching detached joint + * [Pull request #1687](https://github.com/gazebosim/gz-sim/pull/1687) + +1. Enable GzWeb visualization of markers by republishing service requests on a topic + * [Pull request #1994](https://github.com/gazebosim/gz-sim/pull/1994) + +1. Small fixes to gz headers + * [Pull request #1985](https://github.com/gazebosim/gz-sim/pull/1985) + +1. Speed up Resource Spawner load time by fetching model list asynchronously + * [Pull request #1962](https://github.com/gazebosim/gz-sim/pull/1962) + +1. Use ignition::gazebo:: in class instantiation + * [Pull request #1967](https://github.com/gazebosim/gz-sim/pull/1967) + +1. Add missing cmake exports from core library + * [Pull request #1978](https://github.com/gazebosim/gz-sim/pull/1978) + +1. Add tutorial on migrating the Sensor class from gazebo classic + * [Pull request #1930](https://github.com/gazebosim/gz-sim/pull/1930) + +1. Add tutorial on migrating the Actor class from gazebo classic + * [Pull request #1929](https://github.com/gazebosim/gz-sim/pull/1929) + +1. Fix use of actors that only has trajectory animation + * [Pull request #1947](https://github.com/gazebosim/gz-sim/pull/1947) + +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. Infrastructure + * [Pull request #1988](https://github.com/gazebosim/gz-sim/pull/1988) + * [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 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. Add Actor class + * [Pull request #1913](https://github.com/gazebosim/gz-sim/pull/1913) + +1. Add Sensor class + * [Pull request #1912](https://github.com/gazebosim/gz-sim/pull/1912) + +1. Allow to change camera user hfov in camera_view plugin + * [Pull request #1807](https://github.com/gazebosim/gz-sim/pull/1807) + +1. Add Joint class + * [Pull request #1910](https://github.com/gazebosim/gz-sim/pull/1910) + +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. Partial backport of 1728 + * [Pull request #1901](https://github.com/gazebosim/gz-sim/pull/1901) + +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. Backport portion of #1771 to fix command-line test + * [Pull request #1771](https://github.com/gazebosim/gz-sim/pull/1771) + +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. ign -> gz + * [Pull request #1983](https://github.com/gazebosim/gz-sim/pull/1983) + * [Pull request #1646](https://github.com/gazebosim/gz-sim/pull/1646) + * [Pull request #1760](https://github.com/gazebosim/gz-sim/pull/1760) + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1759) + * [Pull request #1758](https://github.com/gazebosim/gz-sim/pull/1758) + * [Pull request #1757](https://github.com/gazebosim/gz-sim/pull/1757) + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1749) + +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. Remove redundant namespace references + * [Pull request #1635](https://github.com/gazebosim/gz-sim/pull/1635) + + ### Gazebo Sim 6.14.0 (2022-12-29) 1. Fix Ackermann plugin zero linVel turningRadius bug @@ -2970,6 +3137,70 @@ ## Gazebo Sim 3.x +### Gazebo Sim 3.15.0 (2023-05-08) + +1. Speed up Resource Spawner load time by fetching model list asynchronously + * [Pull request #1962](https://github.com/gazebosim/gz-sim/pull/1962) + +1. ign -> gz Migrate Ignition Headers : gz-sim + * [Pull request #1646](https://github.com/gazebosim/gz-sim/pull/1646) + * [Pull request #1967](https://github.com/gazebosim/gz-sim/pull/1967) + * [Pull request #1978](https://github.com/gazebosim/gz-sim/pull/1978) + * [Pull request #1983](https://github.com/gazebosim/gz-sim/pull/1983) + * [Pull request #1985](https://github.com/gazebosim/gz-sim/pull/1985) + +1. Infrastructure + * [Pull request #1940](https://github.com/gazebosim/gz-sim/pull/1940) + * [Pull request #1937](https://github.com/gazebosim/gz-sim/pull/1937) + +1. Backport portion of #1771 to fix command-line test + * [Pull request #1771](https://github.com/gazebosim/gz-sim/pull/1771) + +1. cmdsim.rb: fix ruby syntax + * [Pull request #1884](https://github.com/gazebosim/gz-sim/pull/1884) + +1. Fix loading wold with 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 for GUI render event + * [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 in Physics.qml for Component Inspector + * [Pull request #1658](https://github.com/gazebosim/gz-sim/pull/1658) + +1. Convert ignitionrobotics to gazebosim in tutorials + * [Pull request #1757](https://github.com/gazebosim/gz-sim/pull/1757) + * [Pull request #1758](https://github.com/gazebosim/gz-sim/pull/1758) + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1759) + * [Pull request #1760](https://github.com/gazebosim/gz-sim/pull/1760) + +1. Added collection name to About Dialog + * [Pull request #1756](https://github.com/gazebosim/gz-sim/pull/1756) + +1. Remove compiler warnings + * [Pull request #1753](https://github.com/gazebosim/gz-sim/pull/1753) + +1. Update examples to use gazebosim.org + * [Pull request #1749](https://github.com/gazebosim/gz-sim/pull/1749) + +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) + ### Gazebo Sim 3.X.X (20XX-XX-XX) ### Gazebo Sim 3.13.0 (2022-06-01) @@ -3049,7 +3280,7 @@ 1. Fix generation of systems library symlinks in build directory * [Pull request #1160](https://github.com/gazebosim/gz-sim/pull/1160) -1. Backport sim::Util::validTopic() from gz-sim4. +1. Backport sim::Util::validTopic() from ign-gazebo4. * [Pull request #1153](https://github.com/gazebosim/gz-sim/pull/1153) 1. Support setting the background color for sensors diff --git a/Migration.md b/Migration.md index 3ca625815c..746c1ae0ce 100644 --- a/Migration.md +++ b/Migration.md @@ -49,6 +49,11 @@ message's header. * CMake `-config` files * Paths that depend on the project name + * The `Scene3D` plugin has been removed and replaced with `gz-gui`'s `MinimalScene` plugin. See + this same document for the instructions to replace it when it was deprecated 5.x to 6.x. + Setting `false` is no longer required for `TransformControl` and + `ViewAndle` plugins. + * Python library imports such `import ignition.gazebo` and `from ignition import gazebo` should be replaced with `import gz.sim7` and `from gz import sim7`. Note the change from `ignition` to `gz` and the addition of the major version @@ -59,7 +64,6 @@ message's header. + In the Hydrodynamics plugin, inverted the added mass contribution to make it act in the correct direction. - ## Gazebo Sim 6.11.X to 6.12.X * **Modified**: @@ -116,20 +120,23 @@ since pose information is being logged in the `changed_state` topic. * The `GzScene3D` GUI plugin is being deprecated in favor of `MinimalScene`. In order to get the same functionality as `GzScene3D`, users need to add the following plugins: - + `MinimalScene`: base rendering functionality - + `GzSceneManager`: adds / removes / moves entities in the scene - + `EntityContextMenuPlugin`: right-click menu - + `InteractiveViewControl`: orbit controls - + `CameraTracking`: Move to, follow, set camera pose - + `MarkerManager`: Enables the use of markers - + `SelectEntities`: Select entities clicking on the scene - + `Spawn`: Functionality to spawn entities into the scene via GUI - + `VisualizationCapabilities`: View collisions, inertial, CoM, joints, etc. - - Moreover, legacy mode needs to be turned off for the following plugins - for them to work with `MinimalScene` (set `false`): - + `TransformControl`: Translate and rotate - + `ViewAndle`: Move camera to preset angles + + `MinimalScene`: base rendering functionality + + `GzSceneManager`: adds / removes / moves entities in the scene + + `EntityContextMenuPlugin`: right-click menu + + `InteractiveViewControl`: orbit controls + + `CameraTracking`: Move to, follow, set camera pose + + `MarkerManager`: Enables the use of markers + + `SelectEntities`: Select entities clicking on the scene + + `Spawn`: Functionality to spawn entities into the scene via GUI + + `VisualizationCapabilities`: View collisions, inertial, CoM, joints, etc. + + SDF code for all these can be found in: + https://github.com/gazebosim/gz-sim/blob/ff1c82b41e548dfdc8076374f9500db2df2c35a1/examples/worlds/minimal_scene.sdf#L29-L128 + + Moreover, legacy mode needs to be turned off for the following plugins + for them to work with `MinimalScene` (set `false`): + + `TransformControl`: Translate and rotate + + `ViewAndle`: Move camera to preset angles * The `gui.config` and `server.config` files are now located in a versioned folder inside `$HOME/.gz/sim`, i.e. `$HOME/.gz/sim/6/gui.config`. diff --git a/examples/worlds/conveyor.sdf b/examples/worlds/conveyor.sdf index 940dd4af3f..61ba5eb159 100644 --- a/examples/worlds/conveyor.sdf +++ b/examples/worlds/conveyor.sdf @@ -121,6 +121,11 @@ 5 0.2 0.1 + + 0.05 0.05 0.70 1 + 0.05 0.05 0.70 1 + 0.8 0.8 0.8 1 + 2.5 0 0 -1.570796327 0 0 @@ -130,6 +135,11 @@ 0.05 + + 0.05 0.05 0.70 1 + 0.05 0.05 0.70 1 + 0.8 0.8 0.8 1 + -2.5 0 0 -1.570796327 0 0 @@ -139,6 +149,11 @@ 0.05 + + 0.05 0.05 0.70 1 + 0.05 0.05 0.70 1 + 0.8 0.8 0.8 1 + 1 0 @@ -147,6 +162,7 @@ base_link + 1 @@ -157,7 +173,7 @@ 87 - data: 10.0 + data: 1.0 @@ -185,7 +201,7 @@ - 0 0 1 0 0 0 + 0 0 1 0 0 0 1.06 @@ -206,7 +222,9 @@ - 1 1 1 1 + 0.60 0.0 0.0 1 + 0.60 0.0 0.0 1 + 0.8 0.8 0.8 1 @@ -273,6 +291,25 @@ + + + World control + 0 + 0 + 72 + 150 + 1 + floating + + + + + + 1 + 1 + 0 + + diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index f036956dca..1ae187cadb 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/examples/worlds/detachable_joint.sdf @@ -13,6 +13,18 @@ gz topic -t "/B1/detach" -m gz.msgs.Empty -p "unused: true" gz topic -t "/B2/detach" -m gz.msgs.Empty -p "unused: true" gz topic -t "/B3/detach" -m gz.msgs.Empty -p "unused: true" + + To re-attach breadcrumbs + + gz topic -t "/B1/attach" -m gz.msgs.Empty -p "unused: true" + gz topic -t "/B2/attach" -m gz.msgs.Empty -p "unused: true" + gz topic -t "/B3/attach" -m gz.msgs.Empty -p "unused: true" + + To monitor the state of each breadcrumbs + + gz topic -e -t /B1/state + gz topic -e -t /B2/state + gz topic -e -t /B3/state --> @@ -373,23 +385,32 @@ 1.25 0.3 - + chassis B1 body - /B1/detach + /B1/detach + /B1/attach + /B1/state - + chassis B2 body - /B2/detach + /B2/detach + /B2/attach + /B2/state - + chassis B3 body - /B3/detach + /B3/detach + /B3/attach + /B3/state diff --git a/examples/worlds/triggered_publisher.sdf b/examples/worlds/triggered_publisher.sdf index 1457f55d9d..19c29680aa 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -411,13 +411,13 @@ start falling. body box1 box_body - /box1/detach + /box1/detach body box2 box_body - /box2/detach + /box2/detach diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index 700fcf89bd..b87ada778c 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -567,6 +567,17 @@ namespace gz public: std::unordered_set ComponentTypesWithPeriodicChanges() const; + /// \brief Get a cache of components with periodic changes. + /// \param[inout] _changes A list of components with the latest periodic + /// changes. If a component has a periodic change, it is added to the + /// hash map. It the component or entity was removed, it is removed from + /// the hashmap. This way the hashmap stores a list of components and + /// entities which have had periodic changes in the past and still + /// exist within the ECM. + /// \sa EntityComponentManager::PeriodicStateFromCache + public: void UpdatePeriodicChangeCache(std::unordered_map>&) const; + /// \brief Set the absolute state of the ECM from a serialized message. /// Entities / components that are in the new state but not in the old /// one will be created. @@ -594,6 +605,19 @@ namespace gz const std::unordered_set &_types = {}, bool _full = false) const; + /// \brief Populate a message with relevant changes to the state given + /// a periodic change cache. + /// \details The header of the message will not be populated, it is the + /// responsibility of the caller to timestamp it before use. Additionally, + /// changes such as addition or removal will not be populated. + /// \param[inout] _state The serialized state message to populate. + /// \param[in] _cache A map of entities and components to serialize. + /// \sa EntityComponenetManager::UpdatePeriodicChangeCache + public: void PeriodicStateFromCache( + msgs::SerializedStateMap &_state, + const std::unordered_map> &_cache) const; + /// \brief Get a message with the serialized state of all entities and /// components that are changing in the current iteration /// diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index f54f432630..d67a17d4de 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -306,6 +306,19 @@ namespace gz const math::Vector3d &_force, const math::Vector3d &_torque) const; + /// \brief Add a wrench expressed in world coordinates and applied to + /// the link at an offset from the link's origin. This wrench + /// is applied for one simulation step. + /// \param[in] _ecm Mutable Entity-component manager. + /// \param[in] _force Force to be applied expressed in world coordinates + /// \param[in] _torque Torque to be applied expressed in world coordinates + /// \param[in] _offset The point of application of the force expressed + /// in the link frame + public: void AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_torque, + const math::Vector3d &_offset) const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index 7ac9d0bbb1..8d1e6ebc80 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -80,13 +80,15 @@ namespace gz /// /// List syntax: *service_name(request_msg_type) : response_msg_type* /// - /// 1. `/world//scene/info(none)` : gz::msgs::Scene + /// 1. `/world//scene/info`(none) : gz::msgs::Scene /// + Returns the current scene information. /// - /// 2. `/gazebo/resource_paths/get` : gz::msgs::StringMsg_V + /// 2. `/gazebo/resource_paths/get`(gz::msgs::Empty) : + /// gz::msgs::StringMsg_V /// + Get list of resource paths. /// - /// 3. `/gazebo/resource_paths/add` : gz::msgs::Empty + /// 3. `/gazebo/resource_paths/add`(gz::msgs::StringMsg_V) : + /// gz::msgs::Empty /// + Add new resource paths. /// /// 4. `/server_control`(gz::msgs::ServerControl) : diff --git a/include/gz/sim/ServerConfig.hh b/include/gz/sim/ServerConfig.hh index f6dc3df92b..32fe1559b2 100644 --- a/include/gz/sim/ServerConfig.hh +++ b/include/gz/sim/ServerConfig.hh @@ -376,14 +376,14 @@ namespace gz UpdatePeriod() const; /// \brief Path to where simulation resources, such as models downloaded - /// from fuel.ignitionrobotics.org, should be stored. + /// from fuel.gazebosim.org, should be stored. /// \return Path to a location on disk. An empty string indicates that /// the default value will be used, which is currently /// ~/.gz/fuel. public: const std::string &ResourceCache() const; /// \brief Set the path to where simulation resources, such as models - /// downloaded from fuel.ignitionrobotics.org, should be stored. + /// downloaded from fuel.gazebosim.org, should be stored. /// \param[in] _path Path to a location on disk. An empty string /// indicates that the default value will be used, which is currently /// ~/.gz/fuel. diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index 41a7ca3dab..0840c2c1c1 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -24,7 +24,9 @@ #include #include +#include #include +#include #include "gz/sim/components/Environment.hh" #include "gz/sim/config.hh" @@ -309,6 +311,11 @@ namespace gz const math::Vector3d& _worldPosition, const std::shared_ptr& _gridField); + /// \brief Load a mesh from a Mesh SDF DOM + /// \param[in] _meshSdf Mesh SDF DOM + /// \return The loaded mesh or null if the mesh can not be loaded. + GZ_SIM_VISIBLE const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf); + /// \brief Environment variable holding resource paths. const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"}; diff --git a/include/gz/sim/rendering/MarkerManager.hh b/include/gz/sim/rendering/MarkerManager.hh index 3bed374c0d..68492b6f9e 100644 --- a/include/gz/sim/rendering/MarkerManager.hh +++ b/include/gz/sim/rendering/MarkerManager.hh @@ -20,6 +20,7 @@ #include #include +#include #include #include "gz/rendering/RenderTypes.hh" diff --git a/include/gz/sim/rendering/WrenchVisualizer.hh b/include/gz/sim/rendering/WrenchVisualizer.hh new file mode 100644 index 0000000000..c0a382467e --- /dev/null +++ b/include/gz/sim/rendering/WrenchVisualizer.hh @@ -0,0 +1,85 @@ +/* + * 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_WRENCHVISUALIZER_HH_ +#define GZ_SIM_WRENCHVISUALIZER_HH_ + +#include + +#include +#include +#include + +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE +{ +namespace detail +{ + /// \brief Creates, deletes, and maintains force and torque visuals + class GZ_SIM_RENDERING_VISIBLE WrenchVisualizer + { + /// \brief Constructor + public: WrenchVisualizer(); + + /// \brief Destructor + public: ~WrenchVisualizer(); + + /// \brief Initialize the Wrench visualizer + /// \param[in] _scene The rendering scene where the visuals are created + /// \return True if the scene is valid + bool Init(rendering::ScenePtr _scene); + + /// \brief Create a new force visual + /// \param[in] _material The material used for the visual + /// \return Pointer to the created ArrowVisual + public: rendering::ArrowVisualPtr CreateForceVisual( + rendering::MaterialPtr _material); + + /// \brief Create a new torque visual + /// \param[in] _material The material used for the visual + /// \return Pointer to the created Visual + public: rendering::VisualPtr CreateTorqueVisual( + rendering::MaterialPtr _material); + + /// \brief Update the visual of a vector to match its direction and position + /// \param[in] _visual Pointer to the vector visual to be updated + /// \param[in] _direction Direction of the vector + /// \param[in] _position Position of the arrow + /// \param[in] _size Size of the arrow in meters + /// \param[in] _tip True if _position specifies the tip of the vector, + /// false if it specifies tha base of the vector + public: void UpdateVectorVisual(rendering::VisualPtr _visual, + const math::Vector3d &_direction, + const math::Vector3d &_position, + const double _size, + const bool _tip = false); + + /// \internal + /// \brief Private data pointer + GZ_UTILS_UNIQUE_IMPL_PTR(dataPtr) + }; +} +} +} +} +#endif diff --git a/include/ignition/gazebo/Conversions.hh b/include/ignition/gazebo/Conversions.hh index f0d023823d..0f5add3d6c 100644 --- a/include/ignition/gazebo/Conversions.hh +++ b/include/ignition/gazebo/Conversions.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Entity.hh b/include/ignition/gazebo/Entity.hh index 780e6d2917..30af5993ff 100644 --- a/include/ignition/gazebo/Entity.hh +++ b/include/ignition/gazebo/Entity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/EntityComponentManager.hh b/include/ignition/gazebo/EntityComponentManager.hh index 8a6884b0b1..5066b76d60 100644 --- a/include/ignition/gazebo/EntityComponentManager.hh +++ b/include/ignition/gazebo/EntityComponentManager.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/EventManager.hh b/include/ignition/gazebo/EventManager.hh index 639c7fa450..cc99c75b80 100644 --- a/include/ignition/gazebo/EventManager.hh +++ b/include/ignition/gazebo/EventManager.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Events.hh b/include/ignition/gazebo/Events.hh index 7589873d10..cc90c49edf 100644 --- a/include/ignition/gazebo/Events.hh +++ b/include/ignition/gazebo/Events.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Link.hh b/include/ignition/gazebo/Link.hh index 956ecc8127..3122dcda65 100644 --- a/include/ignition/gazebo/Link.hh +++ b/include/ignition/gazebo/Link.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Model.hh b/include/ignition/gazebo/Model.hh index 66304aab1d..b309bc985e 100644 --- a/include/ignition/gazebo/Model.hh +++ b/include/ignition/gazebo/Model.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Primitives.hh b/include/ignition/gazebo/Primitives.hh index 8b30ab930f..41c74f8880 100644 --- a/include/ignition/gazebo/Primitives.hh +++ b/include/ignition/gazebo/Primitives.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh index e010ac689b..d51dd3d976 100644 --- a/include/ignition/gazebo/SdfEntityCreator.hh +++ b/include/ignition/gazebo/SdfEntityCreator.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Server.hh b/include/ignition/gazebo/Server.hh index a289f98d48..df05ba03b3 100644 --- a/include/ignition/gazebo/Server.hh +++ b/include/ignition/gazebo/Server.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/ServerConfig.hh b/include/ignition/gazebo/ServerConfig.hh index 4011c28bd0..7a0cea51c2 100644 --- a/include/ignition/gazebo/ServerConfig.hh +++ b/include/ignition/gazebo/ServerConfig.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/System.hh b/include/ignition/gazebo/System.hh index 825c3c312a..fe4601549e 100644 --- a/include/ignition/gazebo/System.hh +++ b/include/ignition/gazebo/System.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/SystemLoader.hh b/include/ignition/gazebo/SystemLoader.hh index d4d40447da..91699610c6 100644 --- a/include/ignition/gazebo/SystemLoader.hh +++ b/include/ignition/gazebo/SystemLoader.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/SystemPluginPtr.hh b/include/ignition/gazebo/SystemPluginPtr.hh index 17a1324b96..81c88c19f7 100644 --- a/include/ignition/gazebo/SystemPluginPtr.hh +++ b/include/ignition/gazebo/SystemPluginPtr.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/TestFixture.hh b/include/ignition/gazebo/TestFixture.hh index 6864e9a4a7..6a6270ae85 100644 --- a/include/ignition/gazebo/TestFixture.hh +++ b/include/ignition/gazebo/TestFixture.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Types.hh b/include/ignition/gazebo/Types.hh index 91a47264f5..3179ac04dd 100644 --- a/include/ignition/gazebo/Types.hh +++ b/include/ignition/gazebo/Types.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/Util.hh b/include/ignition/gazebo/Util.hh index 59e66bc714..2a2fc349e7 100644 --- a/include/ignition/gazebo/Util.hh +++ b/include/ignition/gazebo/Util.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/World.hh b/include/ignition/gazebo/World.hh index 0f96dae7c6..39d9872c0e 100644 --- a/include/ignition/gazebo/World.hh +++ b/include/ignition/gazebo/World.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/apply-link-wrench-system/Export.hh b/include/ignition/gazebo/apply-link-wrench-system/Export.hh new file mode 100644 index 0000000000..271c690afb --- /dev/null +++ b/include/ignition/gazebo/apply-link-wrench-system/Export.hh @@ -0,0 +1,19 @@ +/* + * 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 diff --git a/include/ignition/gazebo/comms/Broker.hh b/include/ignition/gazebo/comms/Broker.hh index c4294d7b00..91437357fe 100644 --- a/include/ignition/gazebo/comms/Broker.hh +++ b/include/ignition/gazebo/comms/Broker.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/comms/ICommsModel.hh b/include/ignition/gazebo/comms/ICommsModel.hh index a87073e68a..de2214ddfb 100644 --- a/include/ignition/gazebo/comms/ICommsModel.hh +++ b/include/ignition/gazebo/comms/ICommsModel.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/comms/MsgManager.hh b/include/ignition/gazebo/comms/MsgManager.hh index c11f4d7e4f..25f1ff139b 100644 --- a/include/ignition/gazebo/comms/MsgManager.hh +++ b/include/ignition/gazebo/comms/MsgManager.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components.hh b/include/ignition/gazebo/components.hh index 1efacbd975..2e08f047bb 100644 --- a/include/ignition/gazebo/components.hh +++ b/include/ignition/gazebo/components.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Actor.hh b/include/ignition/gazebo/components/Actor.hh index 5dd47b643c..9e9a563b79 100644 --- a/include/ignition/gazebo/components/Actor.hh +++ b/include/ignition/gazebo/components/Actor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Actuators.hh b/include/ignition/gazebo/components/Actuators.hh index 4365a764eb..78be04a165 100644 --- a/include/ignition/gazebo/components/Actuators.hh +++ b/include/ignition/gazebo/components/Actuators.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/AirPressureSensor.hh b/include/ignition/gazebo/components/AirPressureSensor.hh index 1c8318cee8..0d7d40f7e9 100644 --- a/include/ignition/gazebo/components/AirPressureSensor.hh +++ b/include/ignition/gazebo/components/AirPressureSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Altimeter.hh b/include/ignition/gazebo/components/Altimeter.hh index 155b513e0a..f4a431429e 100644 --- a/include/ignition/gazebo/components/Altimeter.hh +++ b/include/ignition/gazebo/components/Altimeter.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/AngularAcceleration.hh b/include/ignition/gazebo/components/AngularAcceleration.hh index f17971d84c..aa01292837 100644 --- a/include/ignition/gazebo/components/AngularAcceleration.hh +++ b/include/ignition/gazebo/components/AngularAcceleration.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/AngularVelocity.hh b/include/ignition/gazebo/components/AngularVelocity.hh index f22762fb8d..1585256c35 100644 --- a/include/ignition/gazebo/components/AngularVelocity.hh +++ b/include/ignition/gazebo/components/AngularVelocity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/AngularVelocityCmd.hh b/include/ignition/gazebo/components/AngularVelocityCmd.hh index 2b683475e6..9f0a95ff71 100644 --- a/include/ignition/gazebo/components/AngularVelocityCmd.hh +++ b/include/ignition/gazebo/components/AngularVelocityCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Atmosphere.hh b/include/ignition/gazebo/components/Atmosphere.hh index 544f4d495f..8f739b8181 100644 --- a/include/ignition/gazebo/components/Atmosphere.hh +++ b/include/ignition/gazebo/components/Atmosphere.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/AxisAlignedBox.hh b/include/ignition/gazebo/components/AxisAlignedBox.hh index 039aa31584..b33b0957e7 100644 --- a/include/ignition/gazebo/components/AxisAlignedBox.hh +++ b/include/ignition/gazebo/components/AxisAlignedBox.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/BatteryPowerLoad.hh b/include/ignition/gazebo/components/BatteryPowerLoad.hh index 99f9606b07..1073d23e85 100644 --- a/include/ignition/gazebo/components/BatteryPowerLoad.hh +++ b/include/ignition/gazebo/components/BatteryPowerLoad.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. @@ -13,7 +13,7 @@ * See the License for the specific language governing permissions and * limitations under the License. * -*/ + */ #include #include diff --git a/include/ignition/gazebo/components/BatterySoC.hh b/include/ignition/gazebo/components/BatterySoC.hh index e6eaf398dd..4f2b0d3038 100644 --- a/include/ignition/gazebo/components/BatterySoC.hh +++ b/include/ignition/gazebo/components/BatterySoC.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/BoundingBoxCamera.hh b/include/ignition/gazebo/components/BoundingBoxCamera.hh index 2bb86098d6..14a78a1325 100644 --- a/include/ignition/gazebo/components/BoundingBoxCamera.hh +++ b/include/ignition/gazebo/components/BoundingBoxCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Camera.hh b/include/ignition/gazebo/components/Camera.hh index c4d7bd9928..05a08fac47 100644 --- a/include/ignition/gazebo/components/Camera.hh +++ b/include/ignition/gazebo/components/Camera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/CanonicalLink.hh b/include/ignition/gazebo/components/CanonicalLink.hh index d948ce335a..6dcf94c4a8 100644 --- a/include/ignition/gazebo/components/CanonicalLink.hh +++ b/include/ignition/gazebo/components/CanonicalLink.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/CastShadows.hh b/include/ignition/gazebo/components/CastShadows.hh index b7903aa094..6c8c6e4ee3 100644 --- a/include/ignition/gazebo/components/CastShadows.hh +++ b/include/ignition/gazebo/components/CastShadows.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/CenterOfVolume.hh b/include/ignition/gazebo/components/CenterOfVolume.hh index 2ff7fe2486..9ec4f6bd33 100644 --- a/include/ignition/gazebo/components/CenterOfVolume.hh +++ b/include/ignition/gazebo/components/CenterOfVolume.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ChildLinkName.hh b/include/ignition/gazebo/components/ChildLinkName.hh index 91661856a7..0c69cbcea8 100644 --- a/include/ignition/gazebo/components/ChildLinkName.hh +++ b/include/ignition/gazebo/components/ChildLinkName.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Collision.hh b/include/ignition/gazebo/components/Collision.hh index 4c4164bcbf..1acd2f98b3 100644 --- a/include/ignition/gazebo/components/Collision.hh +++ b/include/ignition/gazebo/components/Collision.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Component.hh b/include/ignition/gazebo/components/Component.hh index 16934b7446..cbf751e92e 100644 --- a/include/ignition/gazebo/components/Component.hh +++ b/include/ignition/gazebo/components/Component.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ContactSensor.hh b/include/ignition/gazebo/components/ContactSensor.hh index 4228636d9a..82ecedef1d 100644 --- a/include/ignition/gazebo/components/ContactSensor.hh +++ b/include/ignition/gazebo/components/ContactSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ContactSensorData.hh b/include/ignition/gazebo/components/ContactSensorData.hh index 49a683749c..0e55e45f3b 100644 --- a/include/ignition/gazebo/components/ContactSensorData.hh +++ b/include/ignition/gazebo/components/ContactSensorData.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/CustomSensor.hh b/include/ignition/gazebo/components/CustomSensor.hh index 491a66d342..aa9e0acc33 100644 --- a/include/ignition/gazebo/components/CustomSensor.hh +++ b/include/ignition/gazebo/components/CustomSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/DepthCamera.hh b/include/ignition/gazebo/components/DepthCamera.hh index 87a98b1c97..a9ae0f7049 100644 --- a/include/ignition/gazebo/components/DepthCamera.hh +++ b/include/ignition/gazebo/components/DepthCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/DetachableJoint.hh b/include/ignition/gazebo/components/DetachableJoint.hh index 8bdd78b6e0..f27304ace4 100644 --- a/include/ignition/gazebo/components/DetachableJoint.hh +++ b/include/ignition/gazebo/components/DetachableJoint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ExternalWorldWrenchCmd.hh b/include/ignition/gazebo/components/ExternalWorldWrenchCmd.hh index cf9325774b..b93046b043 100644 --- a/include/ignition/gazebo/components/ExternalWorldWrenchCmd.hh +++ b/include/ignition/gazebo/components/ExternalWorldWrenchCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Factory.hh b/include/ignition/gazebo/components/Factory.hh index 752e94003a..31d6e27177 100644 --- a/include/ignition/gazebo/components/Factory.hh +++ b/include/ignition/gazebo/components/Factory.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ForceTorque.hh b/include/ignition/gazebo/components/ForceTorque.hh index 9fb7a234c8..059287fa34 100644 --- a/include/ignition/gazebo/components/ForceTorque.hh +++ b/include/ignition/gazebo/components/ForceTorque.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Geometry.hh b/include/ignition/gazebo/components/Geometry.hh index a0e90aa3fd..4f1422854d 100644 --- a/include/ignition/gazebo/components/Geometry.hh +++ b/include/ignition/gazebo/components/Geometry.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/GpuLidar.hh b/include/ignition/gazebo/components/GpuLidar.hh index 99ea4910ea..27d948bb8a 100644 --- a/include/ignition/gazebo/components/GpuLidar.hh +++ b/include/ignition/gazebo/components/GpuLidar.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Gravity.hh b/include/ignition/gazebo/components/Gravity.hh index a30dba52bd..9f3b9b74ec 100644 --- a/include/ignition/gazebo/components/Gravity.hh +++ b/include/ignition/gazebo/components/Gravity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/HaltMotion.hh b/include/ignition/gazebo/components/HaltMotion.hh index 20dc4123d7..c2064af00b 100644 --- a/include/ignition/gazebo/components/HaltMotion.hh +++ b/include/ignition/gazebo/components/HaltMotion.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Imu.hh b/include/ignition/gazebo/components/Imu.hh index 9f467becf5..87d126ac37 100644 --- a/include/ignition/gazebo/components/Imu.hh +++ b/include/ignition/gazebo/components/Imu.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Inertial.hh b/include/ignition/gazebo/components/Inertial.hh index 4f3af29e9a..ea769736b1 100644 --- a/include/ignition/gazebo/components/Inertial.hh +++ b/include/ignition/gazebo/components/Inertial.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Joint.hh b/include/ignition/gazebo/components/Joint.hh index 70bcf2bdb6..9911dd75d2 100644 --- a/include/ignition/gazebo/components/Joint.hh +++ b/include/ignition/gazebo/components/Joint.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointAxis.hh b/include/ignition/gazebo/components/JointAxis.hh index 71e4b2d511..05fe8bfcff 100644 --- a/include/ignition/gazebo/components/JointAxis.hh +++ b/include/ignition/gazebo/components/JointAxis.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointEffortLimitsCmd.hh b/include/ignition/gazebo/components/JointEffortLimitsCmd.hh index 931e242a98..0055a16e7c 100644 --- a/include/ignition/gazebo/components/JointEffortLimitsCmd.hh +++ b/include/ignition/gazebo/components/JointEffortLimitsCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointForce.hh b/include/ignition/gazebo/components/JointForce.hh index 64d7d27a11..2478809360 100644 --- a/include/ignition/gazebo/components/JointForce.hh +++ b/include/ignition/gazebo/components/JointForce.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointForceCmd.hh b/include/ignition/gazebo/components/JointForceCmd.hh index 2aaa329f42..eab7568629 100644 --- a/include/ignition/gazebo/components/JointForceCmd.hh +++ b/include/ignition/gazebo/components/JointForceCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointPosition.hh b/include/ignition/gazebo/components/JointPosition.hh index 385453a611..6113013778 100644 --- a/include/ignition/gazebo/components/JointPosition.hh +++ b/include/ignition/gazebo/components/JointPosition.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointPositionLimitsCmd.hh b/include/ignition/gazebo/components/JointPositionLimitsCmd.hh index b53a973a39..d98d7c62f6 100644 --- a/include/ignition/gazebo/components/JointPositionLimitsCmd.hh +++ b/include/ignition/gazebo/components/JointPositionLimitsCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointPositionReset.hh b/include/ignition/gazebo/components/JointPositionReset.hh index 6d5424b734..dbad467a07 100644 --- a/include/ignition/gazebo/components/JointPositionReset.hh +++ b/include/ignition/gazebo/components/JointPositionReset.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointTransmittedWrench.hh b/include/ignition/gazebo/components/JointTransmittedWrench.hh index a002b650ed..3b605deca8 100644 --- a/include/ignition/gazebo/components/JointTransmittedWrench.hh +++ b/include/ignition/gazebo/components/JointTransmittedWrench.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointType.hh b/include/ignition/gazebo/components/JointType.hh index 485b450997..517330c4e2 100644 --- a/include/ignition/gazebo/components/JointType.hh +++ b/include/ignition/gazebo/components/JointType.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointVelocity.hh b/include/ignition/gazebo/components/JointVelocity.hh index 4d819c179c..338bbb4b9f 100644 --- a/include/ignition/gazebo/components/JointVelocity.hh +++ b/include/ignition/gazebo/components/JointVelocity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointVelocityCmd.hh b/include/ignition/gazebo/components/JointVelocityCmd.hh index ba0303010b..b5ba6d88d8 100644 --- a/include/ignition/gazebo/components/JointVelocityCmd.hh +++ b/include/ignition/gazebo/components/JointVelocityCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh b/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh index 3c4846a200..deb2f0bdee 100644 --- a/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh +++ b/include/ignition/gazebo/components/JointVelocityLimitsCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/JointVelocityReset.hh b/include/ignition/gazebo/components/JointVelocityReset.hh index f188695787..79abfa801c 100644 --- a/include/ignition/gazebo/components/JointVelocityReset.hh +++ b/include/ignition/gazebo/components/JointVelocityReset.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LaserRetro.hh b/include/ignition/gazebo/components/LaserRetro.hh index 8cb6c6f03a..256b122f92 100644 --- a/include/ignition/gazebo/components/LaserRetro.hh +++ b/include/ignition/gazebo/components/LaserRetro.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Level.hh b/include/ignition/gazebo/components/Level.hh index 819141af37..2c3812264e 100644 --- a/include/ignition/gazebo/components/Level.hh +++ b/include/ignition/gazebo/components/Level.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LevelBuffer.hh b/include/ignition/gazebo/components/LevelBuffer.hh index 6ad245dad6..faa59fedf0 100644 --- a/include/ignition/gazebo/components/LevelBuffer.hh +++ b/include/ignition/gazebo/components/LevelBuffer.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LevelEntityNames.hh b/include/ignition/gazebo/components/LevelEntityNames.hh index 2e5eeaad68..5dc8d2ee8f 100644 --- a/include/ignition/gazebo/components/LevelEntityNames.hh +++ b/include/ignition/gazebo/components/LevelEntityNames.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Lidar.hh b/include/ignition/gazebo/components/Lidar.hh index aa9339bbb7..7f058d4072 100644 --- a/include/ignition/gazebo/components/Lidar.hh +++ b/include/ignition/gazebo/components/Lidar.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Light.hh b/include/ignition/gazebo/components/Light.hh index a0a62df91f..d515ccf1bd 100644 --- a/include/ignition/gazebo/components/Light.hh +++ b/include/ignition/gazebo/components/Light.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LightCmd.hh b/include/ignition/gazebo/components/LightCmd.hh index c2cb26dff6..d9a4c56bc3 100644 --- a/include/ignition/gazebo/components/LightCmd.hh +++ b/include/ignition/gazebo/components/LightCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LightType.hh b/include/ignition/gazebo/components/LightType.hh index cb56882847..bb83332ce6 100644 --- a/include/ignition/gazebo/components/LightType.hh +++ b/include/ignition/gazebo/components/LightType.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LinearAcceleration.hh b/include/ignition/gazebo/components/LinearAcceleration.hh index 01563a7de5..c48a5f9e18 100644 --- a/include/ignition/gazebo/components/LinearAcceleration.hh +++ b/include/ignition/gazebo/components/LinearAcceleration.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LinearVelocity.hh b/include/ignition/gazebo/components/LinearVelocity.hh index fd7bd5a263..193a76aa54 100644 --- a/include/ignition/gazebo/components/LinearVelocity.hh +++ b/include/ignition/gazebo/components/LinearVelocity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LinearVelocityCmd.hh b/include/ignition/gazebo/components/LinearVelocityCmd.hh index 6ac97c57a4..662cfa3bf6 100644 --- a/include/ignition/gazebo/components/LinearVelocityCmd.hh +++ b/include/ignition/gazebo/components/LinearVelocityCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LinearVelocitySeed.hh b/include/ignition/gazebo/components/LinearVelocitySeed.hh index 3786c3d347..826b73e6a7 100644 --- a/include/ignition/gazebo/components/LinearVelocitySeed.hh +++ b/include/ignition/gazebo/components/LinearVelocitySeed.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Link.hh b/include/ignition/gazebo/components/Link.hh index 81d6cd6bd9..ff2518e68f 100644 --- a/include/ignition/gazebo/components/Link.hh +++ b/include/ignition/gazebo/components/Link.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LogPlaybackStatistics.hh b/include/ignition/gazebo/components/LogPlaybackStatistics.hh index 3dd3132115..5d6718aa98 100644 --- a/include/ignition/gazebo/components/LogPlaybackStatistics.hh +++ b/include/ignition/gazebo/components/LogPlaybackStatistics.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LogicalAudio.hh b/include/ignition/gazebo/components/LogicalAudio.hh index 24d5d30680..579effe05c 100644 --- a/include/ignition/gazebo/components/LogicalAudio.hh +++ b/include/ignition/gazebo/components/LogicalAudio.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/LogicalCamera.hh b/include/ignition/gazebo/components/LogicalCamera.hh index 4e05761b85..4086290573 100644 --- a/include/ignition/gazebo/components/LogicalCamera.hh +++ b/include/ignition/gazebo/components/LogicalCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/MagneticField.hh b/include/ignition/gazebo/components/MagneticField.hh index fdf16c71f1..f17a736c00 100644 --- a/include/ignition/gazebo/components/MagneticField.hh +++ b/include/ignition/gazebo/components/MagneticField.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Magnetometer.hh b/include/ignition/gazebo/components/Magnetometer.hh index 3b133ec48e..619b1931e6 100644 --- a/include/ignition/gazebo/components/Magnetometer.hh +++ b/include/ignition/gazebo/components/Magnetometer.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Material.hh b/include/ignition/gazebo/components/Material.hh index 7e88bbeed9..4d786db826 100644 --- a/include/ignition/gazebo/components/Material.hh +++ b/include/ignition/gazebo/components/Material.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Model.hh b/include/ignition/gazebo/components/Model.hh index bd7383fbe0..8fe0cef5e4 100644 --- a/include/ignition/gazebo/components/Model.hh +++ b/include/ignition/gazebo/components/Model.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Name.hh b/include/ignition/gazebo/components/Name.hh index f75e3015ce..aa367b9294 100644 --- a/include/ignition/gazebo/components/Name.hh +++ b/include/ignition/gazebo/components/Name.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/NavSat.hh b/include/ignition/gazebo/components/NavSat.hh index b35ee01d70..9dec2250cc 100644 --- a/include/ignition/gazebo/components/NavSat.hh +++ b/include/ignition/gazebo/components/NavSat.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ParentEntity.hh b/include/ignition/gazebo/components/ParentEntity.hh index 79173905eb..c34f961106 100644 --- a/include/ignition/gazebo/components/ParentEntity.hh +++ b/include/ignition/gazebo/components/ParentEntity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ParentLinkName.hh b/include/ignition/gazebo/components/ParentLinkName.hh index cbab2f081f..0a2e942b26 100644 --- a/include/ignition/gazebo/components/ParentLinkName.hh +++ b/include/ignition/gazebo/components/ParentLinkName.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ParticleEmitter.hh b/include/ignition/gazebo/components/ParticleEmitter.hh index 368bfe1f9d..7c86a7a187 100644 --- a/include/ignition/gazebo/components/ParticleEmitter.hh +++ b/include/ignition/gazebo/components/ParticleEmitter.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Performer.hh b/include/ignition/gazebo/components/Performer.hh index dc60e61613..f386b656fb 100644 --- a/include/ignition/gazebo/components/Performer.hh +++ b/include/ignition/gazebo/components/Performer.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/PerformerAffinity.hh b/include/ignition/gazebo/components/PerformerAffinity.hh index 186b834b83..e8dac3722e 100644 --- a/include/ignition/gazebo/components/PerformerAffinity.hh +++ b/include/ignition/gazebo/components/PerformerAffinity.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/PerformerLevels.hh b/include/ignition/gazebo/components/PerformerLevels.hh index 03648c0bd3..0bfc4229ff 100644 --- a/include/ignition/gazebo/components/PerformerLevels.hh +++ b/include/ignition/gazebo/components/PerformerLevels.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Physics.hh b/include/ignition/gazebo/components/Physics.hh index faec6b62b9..ce8d2769db 100644 --- a/include/ignition/gazebo/components/Physics.hh +++ b/include/ignition/gazebo/components/Physics.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/PhysicsCmd.hh b/include/ignition/gazebo/components/PhysicsCmd.hh index cf27eb0671..f2abcac2b9 100644 --- a/include/ignition/gazebo/components/PhysicsCmd.hh +++ b/include/ignition/gazebo/components/PhysicsCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/PhysicsEnginePlugin.hh b/include/ignition/gazebo/components/PhysicsEnginePlugin.hh index 26bffcc5da..7d22f178cc 100644 --- a/include/ignition/gazebo/components/PhysicsEnginePlugin.hh +++ b/include/ignition/gazebo/components/PhysicsEnginePlugin.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Pose.hh b/include/ignition/gazebo/components/Pose.hh index 70ead7afa2..90aaf89598 100644 --- a/include/ignition/gazebo/components/Pose.hh +++ b/include/ignition/gazebo/components/Pose.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/PoseCmd.hh b/include/ignition/gazebo/components/PoseCmd.hh index 98bec934cc..afea27506c 100644 --- a/include/ignition/gazebo/components/PoseCmd.hh +++ b/include/ignition/gazebo/components/PoseCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Recreate.hh b/include/ignition/gazebo/components/Recreate.hh index e9d2ddd421..f11585232a 100644 --- a/include/ignition/gazebo/components/Recreate.hh +++ b/include/ignition/gazebo/components/Recreate.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/RenderEngineGuiPlugin.hh b/include/ignition/gazebo/components/RenderEngineGuiPlugin.hh index 8aa737d4be..8d13081803 100644 --- a/include/ignition/gazebo/components/RenderEngineGuiPlugin.hh +++ b/include/ignition/gazebo/components/RenderEngineGuiPlugin.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/RenderEngineServerHeadless.hh b/include/ignition/gazebo/components/RenderEngineServerHeadless.hh index ae0bd6adba..dc0a7cfa38 100644 --- a/include/ignition/gazebo/components/RenderEngineServerHeadless.hh +++ b/include/ignition/gazebo/components/RenderEngineServerHeadless.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/RenderEngineServerPlugin.hh b/include/ignition/gazebo/components/RenderEngineServerPlugin.hh index a733729949..3cb2025d50 100644 --- a/include/ignition/gazebo/components/RenderEngineServerPlugin.hh +++ b/include/ignition/gazebo/components/RenderEngineServerPlugin.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/RgbdCamera.hh b/include/ignition/gazebo/components/RgbdCamera.hh index 8d790d5da9..dec1b18058 100644 --- a/include/ignition/gazebo/components/RgbdCamera.hh +++ b/include/ignition/gazebo/components/RgbdCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Scene.hh b/include/ignition/gazebo/components/Scene.hh index 89b6bb02f1..4dbd5c91c4 100644 --- a/include/ignition/gazebo/components/Scene.hh +++ b/include/ignition/gazebo/components/Scene.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SegmentationCamera.hh b/include/ignition/gazebo/components/SegmentationCamera.hh index 4769aa70b3..0bcedbce99 100644 --- a/include/ignition/gazebo/components/SegmentationCamera.hh +++ b/include/ignition/gazebo/components/SegmentationCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SelfCollide.hh b/include/ignition/gazebo/components/SelfCollide.hh index 4e59d3ac83..eb984145dd 100644 --- a/include/ignition/gazebo/components/SelfCollide.hh +++ b/include/ignition/gazebo/components/SelfCollide.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SemanticLabel.hh b/include/ignition/gazebo/components/SemanticLabel.hh index 271d5e9e74..985cd6b8d3 100644 --- a/include/ignition/gazebo/components/SemanticLabel.hh +++ b/include/ignition/gazebo/components/SemanticLabel.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Sensor.hh b/include/ignition/gazebo/components/Sensor.hh index b847682495..61ef5e5469 100644 --- a/include/ignition/gazebo/components/Sensor.hh +++ b/include/ignition/gazebo/components/Sensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index 7543a9176f..25f99c7a26 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SlipComplianceCmd.hh b/include/ignition/gazebo/components/SlipComplianceCmd.hh index 088abd3466..38227cc013 100644 --- a/include/ignition/gazebo/components/SlipComplianceCmd.hh +++ b/include/ignition/gazebo/components/SlipComplianceCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SourceFilePath.hh b/include/ignition/gazebo/components/SourceFilePath.hh index 83618aa260..c830539003 100644 --- a/include/ignition/gazebo/components/SourceFilePath.hh +++ b/include/ignition/gazebo/components/SourceFilePath.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SphericalCoordinates.hh b/include/ignition/gazebo/components/SphericalCoordinates.hh index 410e62b829..ef6996f098 100644 --- a/include/ignition/gazebo/components/SphericalCoordinates.hh +++ b/include/ignition/gazebo/components/SphericalCoordinates.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Static.hh b/include/ignition/gazebo/components/Static.hh index e6d09eb770..f2279d7040 100644 --- a/include/ignition/gazebo/components/Static.hh +++ b/include/ignition/gazebo/components/Static.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/SystemPluginInfo.hh b/include/ignition/gazebo/components/SystemPluginInfo.hh index 1fd4e3d103..96e9d65758 100644 --- a/include/ignition/gazebo/components/SystemPluginInfo.hh +++ b/include/ignition/gazebo/components/SystemPluginInfo.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Temperature.hh b/include/ignition/gazebo/components/Temperature.hh index a13434f982..56545ab92d 100644 --- a/include/ignition/gazebo/components/Temperature.hh +++ b/include/ignition/gazebo/components/Temperature.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/TemperatureRange.hh b/include/ignition/gazebo/components/TemperatureRange.hh index ba2c074805..0ca9e81f40 100644 --- a/include/ignition/gazebo/components/TemperatureRange.hh +++ b/include/ignition/gazebo/components/TemperatureRange.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ThermalCamera.hh b/include/ignition/gazebo/components/ThermalCamera.hh index 57750ef889..e94bebfa4e 100644 --- a/include/ignition/gazebo/components/ThermalCamera.hh +++ b/include/ignition/gazebo/components/ThermalCamera.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/ThreadPitch.hh b/include/ignition/gazebo/components/ThreadPitch.hh index 8dc7773f39..301d0bf1b9 100644 --- a/include/ignition/gazebo/components/ThreadPitch.hh +++ b/include/ignition/gazebo/components/ThreadPitch.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Transparency.hh b/include/ignition/gazebo/components/Transparency.hh index 8338de8d8b..c3efbc805c 100644 --- a/include/ignition/gazebo/components/Transparency.hh +++ b/include/ignition/gazebo/components/Transparency.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Visibility.hh b/include/ignition/gazebo/components/Visibility.hh index 8ec5e83e4b..a3f52c491e 100644 --- a/include/ignition/gazebo/components/Visibility.hh +++ b/include/ignition/gazebo/components/Visibility.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Visual.hh b/include/ignition/gazebo/components/Visual.hh index 43b659d732..89b5e0e03a 100644 --- a/include/ignition/gazebo/components/Visual.hh +++ b/include/ignition/gazebo/components/Visual.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/VisualCmd.hh b/include/ignition/gazebo/components/VisualCmd.hh index 405abc6501..bd6a95e618 100644 --- a/include/ignition/gazebo/components/VisualCmd.hh +++ b/include/ignition/gazebo/components/VisualCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Volume.hh b/include/ignition/gazebo/components/Volume.hh index fb205c1ed4..707b59d122 100644 --- a/include/ignition/gazebo/components/Volume.hh +++ b/include/ignition/gazebo/components/Volume.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/WheelSlipCmd.hh b/include/ignition/gazebo/components/WheelSlipCmd.hh index 79c513ea40..8c90b88a7b 100644 --- a/include/ignition/gazebo/components/WheelSlipCmd.hh +++ b/include/ignition/gazebo/components/WheelSlipCmd.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2022 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/Wind.hh b/include/ignition/gazebo/components/Wind.hh index e86429df7a..b8b15af42d 100644 --- a/include/ignition/gazebo/components/Wind.hh +++ b/include/ignition/gazebo/components/Wind.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/WindMode.hh b/include/ignition/gazebo/components/WindMode.hh index ff823d10a2..16dc8fc04e 100644 --- a/include/ignition/gazebo/components/WindMode.hh +++ b/include/ignition/gazebo/components/WindMode.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/components/World.hh b/include/ignition/gazebo/components/World.hh index 3a245b44d4..348e058f58 100644 --- a/include/ignition/gazebo/components/World.hh +++ b/include/ignition/gazebo/components/World.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/gui/Gui.hh b/include/ignition/gazebo/gui/Gui.hh index ccbd2f30b9..51807c3c72 100644 --- a/include/ignition/gazebo/gui/Gui.hh +++ b/include/ignition/gazebo/gui/Gui.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh index 234b78047b..41cfcc0e35 100644 --- a/include/ignition/gazebo/gui/GuiEvents.hh +++ b/include/ignition/gazebo/gui/GuiEvents.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/gui/GuiSystem.hh b/include/ignition/gazebo/gui/GuiSystem.hh index b35b81fa92..0c16df3966 100644 --- a/include/ignition/gazebo/gui/GuiSystem.hh +++ b/include/ignition/gazebo/gui/GuiSystem.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2018 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/physics/Events.hh b/include/ignition/gazebo/physics/Events.hh index a808f19f2f..7c980873e7 100644 --- a/include/ignition/gazebo/physics/Events.hh +++ b/include/ignition/gazebo/physics/Events.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2021 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/rendering/Events.hh b/include/ignition/gazebo/rendering/Events.hh index 5f6e8ed387..2acd63172f 100644 --- a/include/ignition/gazebo/rendering/Events.hh +++ b/include/ignition/gazebo/rendering/Events.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2020 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/rendering/MarkerManager.hh b/include/ignition/gazebo/rendering/MarkerManager.hh index 2397f7bc86..989367fb63 100644 --- a/include/ignition/gazebo/rendering/MarkerManager.hh +++ b/include/ignition/gazebo/rendering/MarkerManager.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/rendering/RenderUtil.hh b/include/ignition/gazebo/rendering/RenderUtil.hh index 781d156d79..ea17e4da91 100644 --- a/include/ignition/gazebo/rendering/RenderUtil.hh +++ b/include/ignition/gazebo/rendering/RenderUtil.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/include/ignition/gazebo/rendering/SceneManager.hh b/include/ignition/gazebo/rendering/SceneManager.hh index e2bb50a68a..d70a9cb94a 100644 --- a/include/ignition/gazebo/rendering/SceneManager.hh +++ b/include/ignition/gazebo/rendering/SceneManager.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index fe188f1c48..37486f94cc 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -160,13 +160,17 @@ gz_add_component(gz ${cli_sources} GET_TARGET_NAME gz_lib_target) target_link_libraries(${gz_lib_target} - PRIVATE + PUBLIC ${PROJECT_LIBRARY_TARGET_NAME} gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-sim${PROJECT_VERSION_MAJOR} gz-sim${PROJECT_VERSION_MAJOR}-gui ) +# Executable target that runs the GUI without ruby for debugging purposes. +add_executable(runGui gz.cc) +target_link_libraries(runGui PRIVATE ${gz_lib_target}) + # Create the library target gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17) gz_add_get_install_prefix_impl(GET_INSTALL_PREFIX_FUNCTION gz::sim::getInstallPrefix diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 99c8a6baec..5ab9b0a635 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -995,6 +995,42 @@ std::unordered_set return periodicComponents; } +///////////////////////////////////////////////// +void EntityComponentManager::UpdatePeriodicChangeCache( + std::unordered_map> &_changes) const +{ + // Get all changes + for (const auto &[componentType, entities] : + this->dataPtr->periodicChangedComponents) + { + _changes[componentType].insert( + entities.begin(), entities.end()); + } + + // Get all removed components + for (const auto &[entity, components] : + this->dataPtr->componentsMarkedAsRemoved) + { + for (const auto &comp : components) + { + _changes[comp].erase(entity); + } + } + + // Get all removed entities + for (const auto &entity : this->dataPtr->toRemoveEntities) { + for ( + auto components = _changes.begin(); + components != _changes.end(); components++) { + // Its ok to leave component entries empty, the serialization + // code will simply ignore it. In any case the number of components + // is limited, so this cache will never grow too large. + components->second.erase(entity); + } + } +} + ///////////////////////////////////////////////// bool EntityComponentManager::HasEntity(const Entity _entity) const { @@ -1710,6 +1746,48 @@ void EntityComponentManager::State( }); } +////////////////////////////////////////////////// +void EntityComponentManager::PeriodicStateFromCache( + msgs::SerializedStateMap &_state, + const std::unordered_map> &_cache) const +{ + for (auto &[typeId, entities] : _cache) { + // Serialize components that have changed + for (auto &entity : entities) { + // Add entity to message if it does not exist + auto entIter = _state.mutable_entities()->find(entity); + if (entIter == _state.mutable_entities()->end()) + { + msgs::SerializedEntityMap ent; + ent.set_id(entity); + (*_state.mutable_entities())[static_cast(entity)] = ent; + entIter = _state.mutable_entities()->find(entity); + } + + // Find the component in the message + auto compIter = entIter->second.mutable_components()->find(typeId); + if (compIter != entIter->second.mutable_components()->end()) + { + // If the component is present we don't need to update it. + continue; + } + + auto compIdx = this->dataPtr->componentTypeIndex[entity][typeId]; + auto &comp = this->dataPtr->componentStorage[entity][compIdx]; + + // Add the component to the message + msgs::SerializedComponent cmp; + cmp.set_type(comp->TypeId()); + std::ostringstream ostr; + comp->Serialize(ostr); + cmp.set_component(ostr.str()); + (*(entIter->second.mutable_components()))[ + static_cast(typeId)] = cmp; + } + } +} + ////////////////////////////////////////////////// void EntityComponentManager::SetState( const msgs::SerializedState &_stateMsg) diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 485a5c272d..bc7f03ff99 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2216,6 +2216,72 @@ TEST_P(EntityComponentManagerFixture, Descendants) } } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(UpdatePeriodicChangeCache)) +{ + Entity e1 = manager.CreateEntity(); + auto c1 = manager.CreateComponent(e1, IntComponent(123)); + + std::unordered_map> changeTracker; + + // No periodic changes keep cache empty. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 0u); + + // Create a periodic change. + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); + + // 1 periodic change, should be reflected in cache. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u); + + manager.RunSetAllComponentsUnchanged(); + + // Has periodic change. Cache should be full. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].size(), 1u); + + // Serialize state + msgs::SerializedStateMap state; + manager.PeriodicStateFromCache(state, changeTracker); + EXPECT_EQ(state.entities().size(), 1u); + EXPECT_EQ( + state.entities().find(e1)->second.components().size(), 1u); + EXPECT_NE(state.entities().find(e1)->second + .components().find(c1->TypeId()), + state.entities().find(e1)->second.components().end()); + + // Component removed cache should be updated. + manager.RemoveComponent(e1); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].size(), 0u); + + manager.RunSetAllComponentsUnchanged(); + + // Add another component to the entity + auto c2 = manager.CreateComponent(e1, IntComponent(123)); + manager.UpdatePeriodicChangeCache(changeTracker); + + // Cache does not track additions, only PeriodicChanges + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u); + + // Track change + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 1u); + + // Entity removed cache should be updated. + manager.RequestRemoveEntity(e1); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u); +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(SetChanged)) diff --git a/src/LevelManager.cc b/src/LevelManager.cc index c5ae5e4926..e4091bace3 100644 --- a/src/LevelManager.cc +++ b/src/LevelManager.cc @@ -38,14 +38,14 @@ #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" -#include "gz/sim/components/Name.hh" #include "gz/sim/components/LevelBuffer.hh" #include "gz/sim/components/LevelEntityNames.hh" +#include "gz/sim/components/Light.hh" #include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/components/LinearVelocitySeed.hh" #include "gz/sim/components/MagneticField.hh" +#include "gz/sim/components/Model.hh" +#include "gz/sim/components/Name.hh" #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Performer.hh" #include "gz/sim/components/PerformerLevels.hh" diff --git a/src/Link.cc b/src/Link.cc index bf58837f29..8378661dbe 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -412,23 +412,46 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, const math::Vector3d &_force, const math::Vector3d &_torque) const { - auto linkWrenchComp = - _ecm.Component(this->dataPtr->id); + this->AddWorldWrench(_ecm, _force, _torque, math::Vector3d::Zero); +} + +////////////////////////////////////////////////// +void Link::AddWorldWrench(EntityComponentManager &_ecm, + const math::Vector3d &_force, + const math::Vector3d &_torque, + const math::Vector3d &_offset) const +{ + math::Pose3d linkWorldPose; + auto worldPoseComp = _ecm.Component(this->dataPtr->id); + if (worldPoseComp) + { + linkWorldPose = worldPoseComp->Data(); + } + else + { + linkWorldPose = worldPose(this->dataPtr->id, _ecm); + } - components::ExternalWorldWrenchCmd wrench; + // We want the force to be applied at an offset from the link origin, so we + // must compute the resulting force and torque on the link origin. + auto posComWorldCoord = linkWorldPose.Rot().RotateVector(_offset); + math::Vector3d torqueWithOffset = _torque + posComWorldCoord.Cross(_force); + auto linkWrenchComp = + _ecm.Component(this->dataPtr->id); if (!linkWrenchComp) { + components::ExternalWorldWrenchCmd wrench; msgs::Set(wrench.Data().mutable_force(), _force); - msgs::Set(wrench.Data().mutable_torque(), _torque); + msgs::Set(wrench.Data().mutable_torque(), torqueWithOffset); _ecm.CreateComponent(this->dataPtr->id, wrench); } else { msgs::Set(linkWrenchComp->Data().mutable_force(), - msgs::Convert(linkWrenchComp->Data().force()) + _force); + msgs::Convert(linkWrenchComp->Data().force()) + _force); msgs::Set(linkWrenchComp->Data().mutable_torque(), - msgs::Convert(linkWrenchComp->Data().torque()) + _torque); + msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset); } } diff --git a/src/SimulationRunner.cc b/src/SimulationRunner.cc index 8682710bf5..650fe35087 100644 --- a/src/SimulationRunner.cc +++ b/src/SimulationRunner.cc @@ -453,13 +453,23 @@ void SimulationRunner::PublishStats() this->rootClockPub.Publish(clockMsg); } +namespace { + +// Create an sdf::ElementPtr that corresponds to an empty `` element. +sdf::ElementPtr createEmptyPluginElement() +{ + auto plugin = std::make_shared(); + sdf::initFile("plugin.sdf", plugin); + return plugin; +} +} ////////////////////////////////////////////////// void SimulationRunner::AddSystem(const SystemPluginPtr &_system, std::optional _entity, std::optional> _sdf) { auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); - auto sdf = _sdf.value_or(this->sdfWorld->Element()); + auto sdf = _sdf.value_or(createEmptyPluginElement()); this->systemMgr->AddSystem(_system, entity, sdf); } @@ -470,7 +480,7 @@ void SimulationRunner::AddSystem( std::optional> _sdf) { auto entity = _entity.value_or(worldEntity(this->entityCompMgr)); - auto sdf = _sdf.value_or(this->sdfWorld->Element()); + auto sdf = _sdf.value_or(createEmptyPluginElement()); this->systemMgr->AddSystem(_system, entity, sdf); } diff --git a/src/TestFixture.cc b/src/TestFixture.cc index 34626f5c04..70b3959760 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -210,5 +210,12 @@ TestFixture &TestFixture::OnPostUpdate(std::function TestFixture::Server() const { + if (!this->dataPtr->finalized) + { + gzwarn << "Fixture has not been finalized, any functions you attempted" + << "to hook into will not be run. It is recommended to call Finalize()" + << "before accessing the server." + << std::endl; + } return this->dataPtr->server; } diff --git a/src/TestFixture_TEST.cc b/src/TestFixture_TEST.cc index e4538846f0..f810ae64c1 100644 --- a/src/TestFixture_TEST.cc +++ b/src/TestFixture_TEST.cc @@ -42,7 +42,7 @@ class TestFixtureTest : public InternalFixture<::testing::Test> EXPECT_EQ(worldEntity, _entity); ASSERT_NE(nullptr, _sdf); - EXPECT_EQ("world", _sdf->GetName()); + EXPECT_EQ("plugin", _sdf->GetName()); EXPECT_NE(kNullEntity, _ecm.EntityByComponents(components::Name("box"))); EXPECT_NE(kNullEntity, _ecm.EntityByComponents(components::Name("sphere"))); diff --git a/src/Util.cc b/src/Util.cc index db9918272f..e23013c241 100644 --- a/src/Util.cc +++ b/src/Util.cc @@ -18,7 +18,10 @@ #include #include +#include +#include #include +#include #include #include #include @@ -837,6 +840,44 @@ std::string resolveSdfWorldFile(const std::string &_sdfFile, return filePath; } + +const common::Mesh *loadMesh(const sdf::Mesh &_meshSdf) +{ + const common::Mesh *mesh = nullptr; + auto &meshManager = *common::MeshManager::Instance(); + if (common::URI(_meshSdf.Uri()).Scheme() == "name") + { + // if it has a name:// scheme, see if the mesh + // exists in the mesh manager and load it by name + const std::string basename = common::basename(_meshSdf.Uri()); + mesh = meshManager.MeshByName(basename); + if (nullptr == mesh) + { + gzwarn << "Failed to load mesh by name [" << basename + << "]." << std::endl; + return nullptr; + } + } + else if (meshManager.IsValidFilename(_meshSdf.Uri())) + { + // load mesh by file path + auto fullPath = asFullPath(_meshSdf.Uri(), _meshSdf.FilePath()); + mesh = meshManager.Load(fullPath); + if (nullptr == mesh) + { + gzwarn << "Failed to load mesh from [" << fullPath + << "]." << std::endl; + return nullptr; + } + } + else + { + gzwarn << "Failed to load mesh [" << _meshSdf.Uri() + << "]." << std::endl; + return nullptr; + } + return mesh; +} } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index d4bd50edf8..6cc56dbd1b 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -40,6 +40,7 @@ #include "gz/sim/Util.hh" #include "helpers/EnvTestFixture.hh" +#include "test_config.hh" using namespace gz; using namespace sim; @@ -1003,3 +1004,23 @@ TEST_F(UtilTest, ResolveSdfWorldFile) // A bad relative path should return an empty string EXPECT_TRUE(resolveSdfWorldFile("../invalid/does_not_exist.sdf").empty()); } + +///////////////////////////////////////////////// +TEST_F(UtilTest, LoadMesh) +{ + sdf::Mesh meshSdf; + EXPECT_EQ(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("invalid_uri"); + meshSdf.SetFilePath("invalid_filepath"); + EXPECT_EQ(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("name://unit_box"); + EXPECT_NE(nullptr, loadMesh(meshSdf)); + + meshSdf.SetUri("duck.dae"); + std::string filePath = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "media", "duck.dae"); + meshSdf.SetFilePath(filePath); + EXPECT_NE(nullptr, loadMesh(meshSdf)); +} diff --git a/src/gui/Gui.cc b/src/gui/Gui.cc index ef95a468d3..17df64864c 100644 --- a/src/gui/Gui.cc +++ b/src/gui/Gui.cc @@ -439,8 +439,10 @@ std::unique_ptr createGui( "VisualizationCapabilities"}; std::string msg{ - "The [GzScene3D] GUI plugin has been removed since Garden. " - "Loading the following plugins instead:\n"}; + "The [GzScene3D] GUI plugin has been removed since Garden.\n" + "SDF code to replace GzScene3D is available at " + "https://github.com/gazebosim/gz-sim/blob/gz-sim7/Migration.md\n" + "Loading the following plugins instead:\n"}; for (auto extra : extras) { diff --git a/src/gui/plugins/CMakeLists.txt b/src/gui/plugins/CMakeLists.txt index 2a837a3e0e..abc3d9666f 100644 --- a/src/gui/plugins/CMakeLists.txt +++ b/src/gui/plugins/CMakeLists.txt @@ -131,6 +131,7 @@ add_subdirectory(modules) # Plugins add_subdirectory(align_tool) +add_subdirectory(apply_force_torque) add_subdirectory(banana_for_scale) add_subdirectory(component_inspector) add_subdirectory(component_inspector_editor) @@ -141,6 +142,7 @@ add_subdirectory(environment_loader) add_subdirectory(environment_visualization) add_subdirectory(joint_position_controller) add_subdirectory(lights) +add_subdirectory(mouse_drag) add_subdirectory(playback_scrubber) add_subdirectory(plot_3d) add_subdirectory(plotting) diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc new file mode 100644 index 0000000000..24a607d20f --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -0,0 +1,980 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ApplyForceTorque.hh" + +namespace gz +{ +namespace sim +{ + /// \enum RotationToolVector + /// \brief Unique identifiers for which vector is currently being + /// modified by the rotation tool + enum RotationToolVector + { + /// \brief Rotation tool is inactive + NONE = 0, + /// \brief Force vector + FORCE = 1, + /// \brief Torque vector + TORQUE = 2, + }; + + class ApplyForceTorquePrivate + { + /// \brief Publish EntityWrench messages in order to apply force and torque + /// \param[in] _applyForce True if the force should be applied + /// \param[in] _applyTorque True if the torque should be applied + public: void PublishWrench(bool _applyForce, bool _applyTorque); + + /// \brief Perform rendering calls in the rendering thread. + public: void OnRender(); + + /// \brief Update visuals for force and torque + public: void UpdateVisuals(); + + /// \brief Handle mouse events + public: void HandleMouseEvents(); + + /// \brief Transport node + public: transport::Node node; + + /// \brief Publisher for EntityWrench messages + public: transport::Node::Publisher pub; + + /// \brief World name + public: std::string worldName; + + /// \brief True if the ApplyLinkWrench system is loaded + public: bool systemLoaded{false}; + + /// \brief To synchronize member access + public: std::mutex mutex; + + /// \brief Name of the selected model + public: QString modelName; + + /// \brief List of the name of the links in the selected model + public: QStringList linkNameList; + + /// \brief Index of selected link in list + public: int linkIndex{-1}; + + /// \brief Entity of the currently selected Link + public: std::optional selectedEntity; + + /// \brief True if a new entity was selected + public: bool changedEntity{false}; + + /// \brief True if a new link was selected from the dropdown + public: bool changedIndex{false}; + + /// \brief Force to be applied in link-fixed frame + public: math::Vector3d force{0.0, 0.0, 0.0}; + + /// \brief Offset of force application point in link-fixed frame + /// relative to the center of mass + public: math::Vector3d offset{0.0, 0.0, 0.0}; + + /// \brief Torque to be applied in link-fixed frame + public: math::Vector3d torque{0.0, 0.0, 0.0}; + + /// \brief Pose of the link-fixed frame + public: math::Pose3d linkWorldPose; + + /// \brief Pose of the inertial frame relative to the link frame + public: math::Pose3d inertialPose; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Ray used for checking intersection with planes for computing + /// 3d world coordinates from 2d + public: rendering::RayQueryPtr ray{nullptr}; + + /// \brief True if there are new mouse events to process. + public: bool mouseDirty{false}; + + /// \brief True if the rotation tool modified the force or torque vector + public: bool vectorDirty{false}; + + /// \brief Whether the transform gizmo is being dragged. + public: bool transformActive{false}; + + /// \brief Block orbit + public: bool blockOrbit{false}; + + /// \brief True if BlockOrbit events should be sent + public: bool sendBlockOrbit{false}; + + /// \brief Where the mouse left off + public: math::Vector2i mousePressPos = math::Vector2i::Zero; + + /// \brief Holds the latest mouse event + public: gz::common::MouseEvent mouseEvent; + + /// \brief Vector (force or torque) currently being rotated + /// by the rotation tool + public: RotationToolVector activeVector{RotationToolVector::NONE}; + + /// \brief Vector value on start of rotation tool transformation, + /// relative to link + public: math::Vector3d initialVector; + + /// \brief Vector orientation on start of rotation tool transformation, + /// relative to link + public: math::Quaterniond initialVectorRot; + + /// \brief Current orientation of the transformed vector relative to link + public: math::Quaterniond vectorRot; + + /// \brief Active transformation axis on the rotation tool + public: rendering::TransformAxis activeAxis{rendering::TA_NONE}; + + /// \brief Wrench visualizer + public: detail::WrenchVisualizer wrenchVis; + + /// \brief Arrow for visualizing force. + public: rendering::ArrowVisualPtr forceVisual{nullptr}; + + /// \brief Arrow for visualizing torque. + public: rendering::VisualPtr torqueVisual{nullptr}; + + /// \brief Gizmo visual for rotating vectors in rotation tool + public: rendering::GizmoVisualPtr gizmoVisual{nullptr}; + }; +} +} + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +ApplyForceTorque::ApplyForceTorque() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +ApplyForceTorque::~ApplyForceTorque() +{ + if (!this->dataPtr->scene) + return; + this->dataPtr->scene->DestroyNode(this->dataPtr->forceVisual, true); + this->dataPtr->scene->DestroyNode(this->dataPtr->torqueVisual, true); + this->dataPtr->scene->DestroyNode(this->dataPtr->gizmoVisual, true); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::LoadConfig(const tinyxml2::XMLElement */*_pluginElem*/) +{ + if (this->title.empty()) + this->title = "Apply force and torque"; + + // Create wrench publisher + auto worldNames = gz::gui::worldNames(); + if (!worldNames.empty()) + { + this->dataPtr->worldName = worldNames[0].toStdString(); + auto topic = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/wrench"); + if (topic == "") + { + gzerr << "Unable to create publisher" << std::endl; + return; + } + this->dataPtr->pub = + this->dataPtr->node.Advertise(topic); + gzdbg << "Created publisher to " << topic << std::endl; + } + + gz::gui::App()->findChild + ()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool ApplyForceTorque::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + + if (this->dataPtr->vectorDirty) + { + this->dataPtr->vectorDirty = false; + if (this->dataPtr->activeVector == RotationToolVector::FORCE) + emit this->ForceChanged(); + else if (this->dataPtr->activeVector == RotationToolVector::TORQUE) + emit this->TorqueChanged(); + } + } + else if (_event->type() == gz::sim::gui::events::EntitiesSelected::kType) + { + if (!this->dataPtr->blockOrbit && !this->dataPtr->mouseEvent.Dragging()) + { + gz::sim::gui::events::EntitiesSelected *_e = + static_cast(_event); + this->dataPtr->selectedEntity = _e->Data().front(); + this->dataPtr->changedEntity = true; + } + } + else if (_event->type() == gz::sim::gui::events::DeselectAllEntities::kType) + { + if (!this->dataPtr->blockOrbit && !this->dataPtr->mouseEvent.Dragging()) + { + this->dataPtr->selectedEntity.reset(); + this->dataPtr->changedEntity = true; + } + } + else if (_event->type() == gz::gui::events::LeftClickOnScene::kType) + { + gz::gui::events::LeftClickOnScene *_e = + static_cast(_event); + this->dataPtr->mouseEvent = _e->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::MousePressOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::DragOnScene::kType) + { + auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::Update(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + std::lock_guard lock(this->dataPtr->mutex); + + // Load the ApplyLinkWrench system + // TODO(anyone): should this be checked on every Update instead? + if (!this->dataPtr->systemLoaded) + { + const std::string name{"gz::sim::systems::ApplyLinkWrench"}; + const std::string filename{"gz-sim-apply-link-wrench-system"}; + const std::string innerxml{"0"}; + + // Get world entity + Entity worldEntity; + _ecm.Each( + [&](const Entity &_entity, + const components::World */*_world*/, + const components::Name *_name)->bool + { + if (_name->Data() == this->dataPtr->worldName) + { + worldEntity = _entity; + return false; + } + return true; + }); + + // Check if already loaded + auto msg = _ecm.ComponentData(worldEntity); + if (!msg) + { + gzdbg << "Unable to find SystemPluginInfo component for entity " + << worldEntity << std::endl; + return; + } + for (const auto &plugin : msg->plugins()) + { + if (plugin.filename() == filename) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system already loaded" << std::endl; + break; + } + } + + // Request to load system + if (!this->dataPtr->systemLoaded) + { + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(worldEntity); + auto plugin = req.add_plugins(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + unsigned int timeout = 5000; + std::string service{"/world/" + this->dataPtr->worldName + + "/entity/system/add"}; + if (this->dataPtr->node.Request(service, req, timeout, res, result)) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system has been loaded" << std::endl; + } + else + { + gzerr << "Error adding new system to entity: " + << worldEntity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + } + } + } + + if (this->dataPtr->changedEntity) + { + this->dataPtr->changedEntity = false; + + this->dataPtr->modelName = ""; + this->dataPtr->linkNameList.clear(); + this->dataPtr->linkIndex = -1; + + if (this->dataPtr->selectedEntity.has_value()) + { + Model parentModel(*this->dataPtr->selectedEntity); + Link selectedLink(*this->dataPtr->selectedEntity); + if (parentModel.Valid(_ecm)) + { + selectedLink = Link(parentModel.CanonicalLink(_ecm)); + } + else if (selectedLink.Valid(_ecm)) + { + parentModel = *selectedLink.ParentModel(_ecm); + } + else + { + this->dataPtr->selectedEntity.reset(); + return; + } + + this->dataPtr->modelName = QString::fromStdString( + parentModel.Name(_ecm)); + this->dataPtr->selectedEntity = selectedLink.Entity(); + + // Put all of the model's links into the list + auto links = parentModel.Links(_ecm); + unsigned int i{0}; + while (i < links.size()) + { + Link link(links[i]); + this->dataPtr->linkNameList.push_back( + QString::fromStdString(*link.Name(_ecm))); + if (link.Entity() == this->dataPtr->selectedEntity) + { + this->dataPtr->linkIndex = i; + } + ++i; + } + } + } + + if (this->dataPtr->changedIndex) + { + this->dataPtr->changedIndex = false; + + if (this->dataPtr->selectedEntity.has_value()) + { + auto parentModel = Link(*this->dataPtr->selectedEntity).ParentModel(_ecm); + std::string linkName = + this->dataPtr->linkNameList[this->dataPtr->linkIndex].toStdString(); + this->dataPtr->selectedEntity = parentModel->LinkByName(_ecm, linkName); + } + } + + // Get the position of the center of mass and link orientation + if (this->dataPtr->selectedEntity.has_value()) + { + auto linkWorldPose = worldPose(*this->dataPtr->selectedEntity, _ecm); + auto inertial = _ecm.Component( + *this->dataPtr->selectedEntity); + if (inertial) + { + this->dataPtr->linkWorldPose = linkWorldPose; + this->dataPtr->inertialPose = inertial->Data().Pose(); + } + } + + emit this->ModelNameChanged(); + emit this->LinkNameListChanged(); + emit this->LinkIndexChanged(); +} + +///////////////////////////////////////////////// +QString ApplyForceTorque::ModelName() const +{ + return this->dataPtr->modelName; +} + +///////////////////////////////////////////////// +QStringList ApplyForceTorque::LinkNameList() const +{ + return this->dataPtr->linkNameList; +} + +///////////////////////////////////////////////// +int ApplyForceTorque::LinkIndex() const +{ + return this->dataPtr->linkIndex; +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetLinkIndex(int _linkIndex) +{ + this->dataPtr->linkIndex = _linkIndex; + this->dataPtr->changedIndex = true; +} + +///////////////////////////////////////////////// +QVector3D ApplyForceTorque::Force() const +{ + return QVector3D( + this->dataPtr->force.X(), + this->dataPtr->force.Y(), + this->dataPtr->force.Z()); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetForce(QVector3D _force) +{ + this->dataPtr->force.Set(_force.x(), _force.y(), _force.z()); + // Update rotation tool orientation when force is set by components + if (this->dataPtr->activeVector == RotationToolVector::FORCE + && this->dataPtr->activeAxis == rendering::TransformAxis::TA_NONE) + { + this->dataPtr->vectorRot = math::Matrix4d::LookAt( + -this->dataPtr->force, math::Vector3d::Zero).Rotation(); + } + emit this->ForceMagChanged(); +} + +///////////////////////////////////////////////// +double ApplyForceTorque::ForceMag() const +{ + return this->dataPtr->force.Length(); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetForceMag(double _forceMag) +{ + if (this->dataPtr->force == math::Vector3d::Zero) + { + this->dataPtr->force.X() = _forceMag; + } + else + { + this->dataPtr->force = _forceMag * this->dataPtr->force.Normalized(); + } + emit this->ForceChanged(); +} + +///////////////////////////////////////////////// +QVector3D ApplyForceTorque::Torque() const +{ + return QVector3D( + this->dataPtr->torque.X(), + this->dataPtr->torque.Y(), + this->dataPtr->torque.Z()); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetTorque(QVector3D _torque) +{ + this->dataPtr->torque.Set(_torque.x(), _torque.y(), _torque.z()); + // Update rotation tool orientation when torque is set by components + if (this->dataPtr->activeVector == RotationToolVector::TORQUE + && this->dataPtr->activeAxis == rendering::TransformAxis::TA_NONE) + { + this->dataPtr->vectorRot = math::Matrix4d::LookAt( + -this->dataPtr->torque, math::Vector3d::Zero).Rotation(); + } + emit this->TorqueMagChanged(); +} + +///////////////////////////////////////////////// +double ApplyForceTorque::TorqueMag() const +{ + return this->dataPtr->torque.Length(); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::SetTorqueMag(double _torqueMag) +{ + if (this->dataPtr->torque == math::Vector3d::Zero) + { + this->dataPtr->torque.X() = _torqueMag; + } + else + { + this->dataPtr->torque = _torqueMag * this->dataPtr->torque.Normalized(); + } + emit this->TorqueChanged(); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::UpdateOffset(double _x, double _y, double _z) +{ + this->dataPtr->offset.Set(_x, _y, _z); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::ApplyForce() +{ + this->dataPtr->PublishWrench(true, false); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::ApplyTorque() +{ + this->dataPtr->PublishWrench(false, true); +} + +///////////////////////////////////////////////// +void ApplyForceTorque::ApplyAll() +{ + this->dataPtr->PublishWrench(true, true); +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::PublishWrench(bool _applyForce, bool _applyTorque) +{ + std::lock_guard lock(this->mutex); + + if (!this->selectedEntity.has_value()) + { + gzdbg << "No link selected" << std::endl; + return; + } + + // Force and torque in world coordinates + math::Vector3d forceToApply = _applyForce ? + this->linkWorldPose.Rot().RotateVector(this->force) : + math::Vector3d::Zero; + math::Vector3d torqueToApply = _applyTorque ? + this->linkWorldPose.Rot().RotateVector(this->torque) : + math::Vector3d::Zero; + // The ApplyLinkWrench system takes the offset in the link frame + math::Vector3d offsetToApply = _applyForce ? + this->offset + this->inertialPose.Pos() : + math::Vector3d::Zero; + + msgs::EntityWrench msg; + msg.mutable_entity()->set_id(*this->selectedEntity); + msgs::Set(msg.mutable_wrench()->mutable_force(), forceToApply); + msgs::Set(msg.mutable_wrench()->mutable_force_offset(), offsetToApply); + msgs::Set(msg.mutable_wrench()->mutable_torque(), torqueToApply); + + this->pub.Publish(msg); +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::OnRender() +{ + if (!this->scene) + { + // Get scene and user camera + this->scene = rendering::sceneFromFirstRenderEngine(); + if (!this->scene) + { + return; + } + + for (unsigned int i = 0; i < this->scene->SensorCount(); ++i) + { + auto cam = std::dynamic_pointer_cast( + this->scene->SensorByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + gzdbg << "ApplyForceTorque plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + this->ray = this->scene->CreateRayQuery(); + + // Make material into overlay + auto mat = this->scene->Material("Default/TransRed")->Clone(); + mat->SetDepthCheckEnabled(false); + mat->SetDepthWriteEnabled(false); + + // Create visuals + if (!this->wrenchVis.Init(this->scene)) + { + gzerr << "Invalid scene" << std::endl; + return; + } + this->forceVisual = this->wrenchVis.CreateForceVisual(mat); + this->torqueVisual = this->wrenchVis.CreateTorqueVisual(mat); + this->gizmoVisual = this->scene->CreateGizmoVisual(); + this->scene->RootVisual()->AddChild(this->gizmoVisual); + } + + this->HandleMouseEvents(); + + this->UpdateVisuals(); + + if (this->sendBlockOrbit) + { + // Events with false should only be sent once + if (!this->blockOrbit) + this->sendBlockOrbit = false; + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::UpdateVisuals() +{ + math::Pose3d inertialWorldPose = this->linkWorldPose * this->inertialPose; + // Update force visualization + if (this->force != math::Vector3d::Zero && + this->selectedEntity.has_value()) + { + math::Vector3d worldForce = + this->linkWorldPose.Rot().RotateVector(this->force); + math::Vector3d applicationPoint = + inertialWorldPose.Pos() + + this->linkWorldPose.Rot().RotateVector(this->offset); + double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) + / 2.0; + this->wrenchVis.UpdateVectorVisual( + this->forceVisual, worldForce, applicationPoint, scale, true); + this->forceVisual->SetVisible(true); + } + else + { + this->forceVisual->SetVisible(false); + } + + // Update torque visualization + if (this->torque != math::Vector3d::Zero && + this->selectedEntity.has_value()) + { + math::Vector3d worldTorque = + this->linkWorldPose.Rot().RotateVector(this->torque); + math::Vector3d applicationPoint = + inertialWorldPose.Pos(); + double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) + / 2.0; + this->wrenchVis.UpdateVectorVisual( + this->torqueVisual, worldTorque, applicationPoint, scale, false); + this->torqueVisual->SetVisible(true); + } + else + { + this->torqueVisual->SetVisible(false); + } + + // Update gizmo visual + if (this->activeVector != RotationToolVector::NONE) + { + math::Vector3d pos; + if (this->activeVector == RotationToolVector::FORCE + && this->force != math::Vector3d::Zero) + { + pos = + inertialWorldPose.Pos() + + this->linkWorldPose.Rot().RotateVector(this->offset); + } + else if (this->activeVector == RotationToolVector::TORQUE + && this->torque != math::Vector3d::Zero) + { + pos = inertialWorldPose.Pos(); + } + else + { + this->activeVector = RotationToolVector::NONE; + this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_NONE); + this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); + return; + } + + double scale = pos.Distance(this->camera->WorldPose().Pos()) + / 2.0; + + // TODO(anyone): use GizmoVisual::LookAt function from + // https://github.com/gazebosim/gz-rendering/pull/882 + + // Update gizmo visual rotation so that they are always facing the + // eye position and alligned with the active vector + math::Quaterniond gizmoRot = this->linkWorldPose.Rot() * this->vectorRot; + math::Vector3d eye = gizmoRot.RotateVectorReverse( + (this->camera->WorldPosition() - pos).Normalized()); + + rendering::VisualPtr xVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_X); + xVis->SetVisible(false); + + rendering::VisualPtr yVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_Y); + math::Vector3d yRot(0, atan2(eye.X(), eye.Z()), 0); + math::Vector3d yRotOffset(GZ_PI * 0.5, -GZ_PI * 0.5, 0); + yVis->SetWorldRotation(gizmoRot * + math::Quaterniond(yRot) * math::Quaterniond(yRotOffset)); + + rendering::VisualPtr zVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_Z); + math::Vector3d zRot(0, 0, atan2(eye.Y(), eye.X())); + zVis->SetWorldRotation(gizmoRot * math::Quaterniond(zRot)); + + rendering::VisualPtr circleVis = this->gizmoVisual->ChildByAxis( + rendering::TransformAxis::TA_ROTATION_Z << 1); + math::Matrix4d lookAt; + lookAt = lookAt.LookAt(this->camera->WorldPosition(), pos); + math::Vector3d circleRotOffset(0, GZ_PI * 0.5, 0); + circleVis->SetWorldRotation( + lookAt.Rotation() * math::Quaterniond(circleRotOffset)); + + this->gizmoVisual->SetLocalScale(1.5 * scale); + this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_ROTATION); + this->gizmoVisual->SetLocalPosition(pos); + + if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Y) + this->gizmoVisual->SetActiveAxis(math::Vector3d::UnitY); + else if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Z) + this->gizmoVisual->SetActiveAxis(math::Vector3d::UnitZ); + else + this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); + } + else + { + this->gizmoVisual->SetTransformMode(rendering::TransformMode::TM_NONE); + this->gizmoVisual->SetActiveAxis(math::Vector3d::Zero); + } +} + +///////////////////////////////////////////////// +void ApplyForceTorquePrivate::HandleMouseEvents() +{ + // check for mouse events + if (!this->mouseDirty) + return; + this->mouseDirty = false; + + // handle mouse movements + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + { + // Rotating vector around the clicked axis + if (this->mouseEvent.Type() == common::MouseEvent::PRESS + && this->activeVector != RotationToolVector::NONE) + { + this->mousePressPos = this->mouseEvent.Pos(); + + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (visual) + { + // check if the visual is an axis in the gizmo visual + auto axis = this->gizmoVisual->AxisById(visual->Id()); + if (axis == rendering::TransformAxis::TA_ROTATION_Y + || axis == rendering::TransformAxis::TA_ROTATION_Z) + { + if (this->activeVector == RotationToolVector::FORCE) + this->initialVector = this->force; + else if (this->activeVector == RotationToolVector::TORQUE) + this->initialVector = this->torque; + else + return; + this->initialVectorRot = this->vectorRot; + this->blockOrbit = true; + this->sendBlockOrbit = true; + this->activeAxis = axis; + } + else + { + this->blockOrbit = false; + this->sendBlockOrbit = true; + this->activeAxis = rendering::TransformAxis::TA_NONE; + return; + } + } + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->blockOrbit = false; + this->sendBlockOrbit = true; + this->activeAxis = rendering::TransformAxis::TA_NONE; + + if (!this->mouseEvent.Dragging()) + { + // get the visual at mouse position + rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + if (!visual) + return; + + // Select a vector for the rotation tool + auto id = visual->Id(); + if ((this->forceVisual->Id() == id || + this->forceVisual->ChildById(id)) + && this->activeVector != RotationToolVector::FORCE) + { + this->activeVector = RotationToolVector::FORCE; + this->vectorRot = math::Matrix4d::LookAt( + -this->force, math::Vector3d::Zero).Rotation(); + } + else if ((this->torqueVisual->Id() == id || + this->torqueVisual->ChildById(id)) + && this->activeVector != RotationToolVector::TORQUE) + { + this->activeVector = RotationToolVector::TORQUE; + this->vectorRot = math::Matrix4d::LookAt( + -this->torque, math::Vector3d::Zero).Rotation(); + } + else if (this->gizmoVisual->AxisById(visual->Id()) == + rendering::TransformAxis::TA_NONE) + { + this->activeVector = RotationToolVector::NONE; + } + } + } + } + if (this->mouseEvent.Type() == common::MouseEvent::MOVE + && this->activeAxis != rendering::TransformAxis::TA_NONE) + { + this->blockOrbit = true; + this->sendBlockOrbit = true; + + auto imageWidth = static_cast(this->camera->ImageWidth()); + auto imageHeight = static_cast(this->camera->ImageHeight()); + double nx = 2.0 * this->mousePressPos.X() / imageWidth - 1.0; + double ny = 1.0 - 2.0 * this->mousePressPos.Y() / imageHeight; + double nxEnd = 2.0 * this->mouseEvent.Pos().X() / imageWidth - 1.0; + double nyEnd = 1.0 - 2.0 * this->mouseEvent.Pos().Y() / imageHeight; + math::Vector2d start(nx, ny); + math::Vector2d end(nxEnd, nyEnd); + + // Axis of rotation in world frame + math::Vector3d axis; + if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Y) + { + axis = this->linkWorldPose.Rot().RotateVector(this->vectorRot.YAxis()); + } + else if (this->activeAxis == rendering::TransformAxis::TA_ROTATION_Z) + { + axis = this->linkWorldPose.Rot().RotateVector(this->vectorRot.ZAxis()); + } + + /// get start and end pos in world frame from 2d point + math::Pose3d inertialWorldPose = this->linkWorldPose * this->inertialPose; + math::Vector3d pos = inertialWorldPose.Pos() + + this->linkWorldPose.Rot().RotateVector(this->offset); + double d = pos.Dot(axis); + math::Planed plane(axis, d); + + math::Vector3d startPos; + this->ray->SetFromCamera(this->camera, start); + if (auto v{plane.Intersection( + this->ray->Origin(), this->ray->Direction())}) + startPos = *v; + else + return; + + math::Vector3d endPos; + this->ray->SetFromCamera(this->camera, end); + if (auto v{plane.Intersection( + this->ray->Origin(), this->ray->Direction())}) + endPos = *v; + else + return; + + // get vectors from link pos to both points + startPos = (startPos - pos).Normalized(); + endPos = (endPos - pos).Normalized(); + // compute angle between two vectors + double signTest = startPos.Cross(endPos).Dot(axis); + double angle = atan2( + (startPos.Cross(endPos)).Length(), startPos.Dot(endPos)); + if (signTest < 0) + angle = -angle; + + // Desired rotation in link frame + axis = this->linkWorldPose.Rot().RotateVectorReverse(axis); + math::Quaterniond rot(axis, angle); + this->vectorRot = rot * this->initialVectorRot; + math::Vector3d newVector = + this->initialVector.Length() * this->vectorRot.XAxis(); + + if (this->activeVector == RotationToolVector::FORCE) + this->force = newVector; + else if (this->activeVector == RotationToolVector::TORQUE) + this->torque = newVector; + this->vectorDirty = true; + } +} + +// Register this plugin +GZ_ADD_PLUGIN(ApplyForceTorque, gz::gui::Plugin) diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh new file mode 100644 index 0000000000..9c9e219834 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh @@ -0,0 +1,210 @@ +/* + * 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_GUI_APPLYFORCETORQUE_HH_ +#define GZ_GUI_APPLYFORCETORQUE_HH_ + +#include + +#include +#include +#include +#include +#include + +#include +#include + +namespace gz +{ +namespace sim +{ + class ApplyForceTorquePrivate; + + /// \brief Publish wrench to "/world//wrench" topic. + /// Automatically loads the ApplyLinkWrench system. + /// + /// ## Configuration + /// This plugin doesn't accept any custom configuration. + class ApplyForceTorque : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Model name + Q_PROPERTY( + QString modelName + READ ModelName + NOTIFY ModelNameChanged + ) + + /// \brief Link list + Q_PROPERTY( + QStringList linkNameList + READ LinkNameList + NOTIFY LinkNameListChanged + ) + + /// \brief Link index + Q_PROPERTY( + int linkIndex + READ LinkIndex + WRITE SetLinkIndex + NOTIFY LinkIndexChanged + ) + + /// \brief Force + Q_PROPERTY( + QVector3D force + READ Force + WRITE SetForce + NOTIFY ForceChanged + ) + + /// \brief Force magnitude + Q_PROPERTY( + double forceMag + READ ForceMag + WRITE SetForceMag + NOTIFY ForceMagChanged + ) + + /// \brief Torque + Q_PROPERTY( + QVector3D torque + READ Torque + WRITE SetTorque + NOTIFY TorqueChanged + ) + + /// \brief Torque magnitude + Q_PROPERTY( + double torqueMag + READ TorqueMag + WRITE SetTorqueMag + NOTIFY TorqueMagChanged + ) + + /// \brief Constructor + public: ApplyForceTorque(); + + /// \brief Destructor + public: ~ApplyForceTorque() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Get the name of the selected model + /// \return The model name + public: Q_INVOKABLE QString ModelName() const; + + /// \brief Notify that the model name changed + signals: void ModelNameChanged(); + + /// \brief Get the name of the links of the selected model + /// \return The list of link names + public: Q_INVOKABLE QStringList LinkNameList() const; + + /// \brief Notify that the link list changed + signals: void LinkNameListChanged(); + + /// \brief Get index of the link in the list + /// \return The link index + public: Q_INVOKABLE int LinkIndex() const; + + /// \brief Notify that the link index changed + signals: void LinkIndexChanged(); + + /// \brief Set the index of the link in the list + /// \param[in] _linkIndex The new link index + public: Q_INVOKABLE void SetLinkIndex(int _linkIndex); + + /// \brief Get the force vector + /// \return The force vector + public: Q_INVOKABLE QVector3D Force() const; + + /// \brief Notify that the force changed + signals: void ForceChanged(); + + /// \brief Set the force vector + /// \param[in] _force The new force vector + public: Q_INVOKABLE void SetForce(QVector3D _force); + + /// \brief Get the magnitude of the force vector + /// \return The force magnitude + public: Q_INVOKABLE double ForceMag() const; + + /// \brief Notify that the force magnitude changed + signals: void ForceMagChanged(); + + /// \brief Set the magnitude of the force vector, scaling it to + /// keep its direction + /// \param[in] _forceMag The new force magnitude + public: Q_INVOKABLE void SetForceMag(double _forceMag); + + /// \brief Get the torque vector + /// \return The torque vector + public: Q_INVOKABLE QVector3D Torque() const; + + /// \brief Notify that the torque changed + signals: void TorqueChanged(); + + /// \brief Set the torque vector + /// \param[in] _torque The new torque vector + public: Q_INVOKABLE void SetTorque(QVector3D _torque); + + /// \brief Get the magnitude of the torque vector + /// \return The torque magnitude + public: Q_INVOKABLE double TorqueMag() const; + + /// \brief Notify that the torque magnitude changed + signals: void TorqueMagChanged(); + + /// \brief Set the magnitude of the torque vector, scaling it to + /// keep its direction + /// \param[in] _torqueMag The new torque magnitude + public: Q_INVOKABLE void SetTorqueMag(double _torqueMag); + + /// \brief Set components of offset vector + /// \param[in] _x X component of offset + /// \param[in] _y Y component of offset + /// \param[in] _z Z component of offset + public: Q_INVOKABLE void UpdateOffset(double _x, double _y, double _z); + + /// \brief Apply the specified force + public: Q_INVOKABLE void ApplyForce(); + + /// \brief Apply the specified torque + public: Q_INVOKABLE void ApplyTorque(); + + /// \brief Apply the specified force and torque + public: Q_INVOKABLE void ApplyAll(); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml new file mode 100644 index 0000000000..7a24301135 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml @@ -0,0 +1,368 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 750 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + // Maximum value for GzSpinBoxes + property int maxValue: 1000000 + + // Minimum value for GzSpinBoxes + property int minValue: -1000000 + + // Precision of the GzSpinBoxes in decimal places + property int decimalPlaces: 2 + + // Step size of the force and torque + property double step: 100.0 + + // Step size of the offset + property double stepOffset: 1.0 + + Label { + Layout.columnSpan: 8 + Layout.fillWidth: true + wrapMode: Text.WordWrap + id: frameText + text: "Forces and torques are given in link-fixed frame." + } + + Label { + Layout.columnSpan: 8 + Layout.fillWidth: true + wrapMode: Text.WordWrap + id: rotText + text: "Click on an arrow to toggle its rotation tool." + } + + Text { + Layout.columnSpan: 2 + id: modelText + color: "black" + text: qsTr("Model:") + } + + Text { + Layout.columnSpan: 6 + id: modelName + color: "black" + text: ApplyForceTorque.modelName + } + + Text { + Layout.columnSpan: 2 + id: linkText + color: "black" + text: qsTr("Link:") + } + + ComboBox { + Layout.columnSpan: 6 + id: linkCombo + Layout.fillWidth: true + model: ApplyForceTorque.linkNameList + currentIndex: ApplyForceTorque.linkIndex + onCurrentIndexChanged: { + ApplyForceTorque.linkIndex = currentIndex + } + } + + // Force and offset + Text { + Layout.columnSpan: 4 + id: forceText + color: "black" + text: qsTr("Force:") + } + + Text { + Layout.columnSpan: 4 + Layout.fillWidth: true + wrapMode: Text.WordWrap + id: offsetText + color: "black" + text: qsTr("Offset (from COM):") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceXText + color: "black" + text: qsTr("X (N)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: forceX + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.force.x + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.force.x = forceX.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: offsetXText + color: "black" + text: qsTr("X (m)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: offsetX + maximumValue: maxValue + minimumValue: minValue + value: 0 + decimals: decimalPlaces + stepSize: stepOffset + onValueChanged: ApplyForceTorque.UpdateOffset( + offsetX.value, offsetY.value, offsetZ.value) + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceYText + color: "black" + text: qsTr("Y (N)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: forceY + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.force.y + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.force.y = forceY.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: offsetYText + color: "black" + text: qsTr("Y (m)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: offsetY + maximumValue: maxValue + minimumValue: minValue + value: 0 + decimals: decimalPlaces + stepSize: stepOffset + onValueChanged: ApplyForceTorque.UpdateOffset( + offsetX.value, offsetY.value, offsetZ.value) + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceZText + color: "black" + text: qsTr("Z (N)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: forceZ + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.force.z + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.force.z = forceZ.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: offsetZText + color: "black" + text: qsTr("Z (m)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: offsetZ + maximumValue: maxValue + minimumValue: minValue + value: 0 + decimals: decimalPlaces + stepSize: stepOffset + onValueChanged: ApplyForceTorque.UpdateOffset( + offsetX.value, offsetY.value, offsetZ.value) + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: forceMagText + color: "black" + text: qsTr("Mag. (N)") + } + + GzSpinBox { + Layout.columnSpan: 2 + Layout.fillWidth: true + id: forceMag + maximumValue: maxValue + minimumValue: 0 + value: ApplyForceTorque.forceMag + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.forceMag = forceMag.value + } + + Button { + text: qsTr("Apply Force") + Layout.columnSpan: 8 + Layout.fillWidth: true + onClicked: function() { + ApplyForceTorque.ApplyForce() + } + } + + // Torque + Text { + Layout.columnSpan: 8 + id: torqueText + color: "black" + text: qsTr("Torque:") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueXText + color: "black" + text: qsTr("X (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueX + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.torque.x + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torque.x = torqueX.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueYText + color: "black" + text: qsTr("Y (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueY + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.torque.y + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torque.y = torqueY.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueZText + color: "black" + text: qsTr("Z (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueZ + maximumValue: maxValue + minimumValue: minValue + value: ApplyForceTorque.torque.z + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torque.z = torqueZ.value + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: torqueMagText + color: "black" + text: qsTr("Mag. (N.m)") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: torqueMag + maximumValue: maxValue + minimumValue: 0 + value: ApplyForceTorque.torqueMag + decimals: decimalPlaces + stepSize: step + onValueChanged: ApplyForceTorque.torqueMag = torqueMag.value + } + + Button { + text: qsTr("Apply Torque") + Layout.columnSpan: 8 + Layout.fillWidth: true + onClicked: function() { + ApplyForceTorque.ApplyTorque() + } + } + + Button { + text: qsTr("Apply All") + Layout.columnSpan: 8 + Layout.fillWidth: true + onClicked: function() { + ApplyForceTorque.ApplyAll() + } + } +} diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qrc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qrc new file mode 100644 index 0000000000..b95739311d --- /dev/null +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qrc @@ -0,0 +1,5 @@ + + + ApplyForceTorque.qml + + diff --git a/src/gui/plugins/apply_force_torque/CMakeLists.txt b/src/gui/plugins/apply_force_torque/CMakeLists.txt new file mode 100644 index 0000000000..db13a65144 --- /dev/null +++ b/src/gui/plugins/apply_force_torque/CMakeLists.txt @@ -0,0 +1,6 @@ +gz_add_gui_plugin(ApplyForceTorque + SOURCES ApplyForceTorque.cc + QT_HEADERS ApplyForceTorque.hh + PUBLIC_LINK_LIBS + ${PROJECT_LIBRARY_TARGET_NAME}-rendering +) diff --git a/src/gui/plugins/mouse_drag/CMakeLists.txt b/src/gui/plugins/mouse_drag/CMakeLists.txt new file mode 100644 index 0000000000..9d34ef4653 --- /dev/null +++ b/src/gui/plugins/mouse_drag/CMakeLists.txt @@ -0,0 +1,4 @@ +gz_add_gui_plugin(MouseDrag + SOURCES MouseDrag.cc + QT_HEADERS MouseDrag.hh +) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.cc b/src/gui/plugins/mouse_drag/MouseDrag.cc new file mode 100644 index 0000000000..44b0cc3e35 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.cc @@ -0,0 +1,839 @@ +/* + * 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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "MouseDrag.hh" + +namespace gz +{ +namespace sim +{ + /// \enum MouseDragMode + /// \brief Unique identifiers for mouse dragging modes + enum MouseDragMode + { + /// \brief Inactive state + NONE = 0, + /// \brief Rotation mode + ROTATE = 1, + /// \brief Translation mode + TRANSLATE = 2, + }; + + class MouseDragPrivate + { + /// \brief Handle mouse events + public: void HandleMouseEvents(); + + /// \brief Perform rendering calls in the rendering thread. + public: void OnRender(); + + /// \brief Update the gains for the controller + /// \param[in] _inertial Inertial of the link + public: void UpdateGains(const math::Inertiald &_inertial); + + /// \brief Calculate the wrench to be applied to the link, + /// based on a spring-damper control law. + /// + /// This attempts to minimize the error (in position or + /// rotation, depending on the mode) with a proportional control, while + /// damping the rate of change of the link's pose. + /// + /// \param[in] _error Position error in translation mode, Rotation error + /// (in Euler angles) in rotation mode + /// \param[in] _dt Change in time since last call + /// \param[in] _linkWorldPose World pose of the link + /// \param[in] _inertial inertial of the link + /// \return The EntityWrench message containing the calculated + /// force and torque + public: msgs::EntityWrench CalculateWrench( + const math::Vector3d &_error, + const std::chrono::duration &_dt, + const math::Pose3d &_linkWorldPose); + + /// \brief Sets the RayQuery from the user camera to the given position + /// on the screen + /// \param[in] _pos Position on the screen + public: void SetRayFromCamera(const math::Vector2i &_pos); + + /// \brief Corrects an angle so that it is in the [-pi; pi] interval, + /// in order to eliminate the discontinuity around 0 and 2pi + /// \param[in] _angle The angle in radians + /// \return The corrected angle + public: double CorrectAngle(const double _angle); + + /// \brief Transport node + public: transport::Node node; + + /// \brief Publisher for EntityWrench messages + public: transport::Node::Publisher pub; + + /// \brief To synchronize member access + public: std::mutex mutex; + + /// \brief World name + public: std::string worldName; + + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; + + /// \brief User camera + public: rendering::CameraPtr camera{nullptr}; + + /// \brief Ray query for mouse clicks + public: rendering::RayQueryPtr rayQuery{nullptr}; + + /// \brief Holds the latest mouse event + public: gz::common::MouseEvent mouseEvent; + + /// \brief Initial position of a mouse click + public: math::Vector2i mousePressPos; + + /// \brief True if there are new mouse events to process + public: bool mouseDirty{false}; + + /// \brief True if there are new dragging mode changes to process + public: bool modeDirty{false}; + + /// \brief True if the force should be applied to the center of mass + public: bool applyCOM{false}; + + /// \brief True if camera orbit should be blocked + public: bool blockOrbit{false}; + + /// \brief True if BlockOrbit events should be sent + public: bool sendBlockOrbit{false}; + + /// \brief True if dragging is active + public: bool dragActive{false}; + + /// \brief True if the ApplyLinkWrench system is loaded + public: bool systemLoaded{false}; + + /// \brief Current dragging mode + public: MouseDragMode mode = MouseDragMode::NONE; + + /// \brief Link to which the wrenches are applied + public: Entity linkId{kNullEntity}; + + /// \brief Plane of force application + public: math::Planed plane; + + /// \brief Application point of the wrench in world coordinates + public: math::Vector3d applicationPoint; + + /// \brief Initial world rotation of the link during mouse click + public: math::Quaterniond initialRot; + + /// \brief Point to which the link is dragged in translation mode + public: math::Vector3d target; + + /// \brief Goal link rotation for rotation mode + public: math::Quaterniond goalRot; + + /// \brief Offset of the force application point relative to the + /// link origin, expressed in the link-fixed frame + public: math::Vector3d offset{0.0, 0.0, 0.0}; + + /// \brief Link world pose in previous Update call. Used for damping + public: math::Pose3d poseLast; + + /// \brief Spring stiffness for translation, in (m/s²)/m + public: double posStiffness{100.0}; + + /// \brief P-gain for translation + public: double pGainPos{0.0}; + + /// \brief D-gain for translation + public: double dGainPos{0.0}; + + /// \brief Spring stiffness for rotation, in (rad/s²)/rad + public: double rotStiffness{200.0}; + + /// \brief P-gain for rotation + public: double pGainRot{0.0}; + + /// \brief D-gain for rotation + public: double dGainRot{0.0}; + + /// \brief Arrow for visualizing force in translation mode. + /// This arrow goes from the application point to the target point. + public: rendering::ArrowVisualPtr arrowVisual{nullptr}; + + /// \brief Box for visualizing rotation mode + public: rendering::VisualPtr boxVisual{nullptr}; + + /// \brief Size of the bounding box of the selected link + public: math::Vector3d bboxSize; + }; +} +} + +using namespace gz; +using namespace sim; + +///////////////////////////////////////////////// +MouseDrag::MouseDrag() + : GuiSystem(), dataPtr(std::make_unique()) +{ +} + +///////////////////////////////////////////////// +MouseDrag::~MouseDrag() = default; + +///////////////////////////////////////////////// +void MouseDrag::LoadConfig(const tinyxml2::XMLElement *_pluginElem) +{ + if (this->title.empty()) + this->title = "Mouse drag"; + + // Create wrench publisher + const auto worldNames = gz::gui::worldNames(); + if (!worldNames.empty()) + { + this->dataPtr->worldName = worldNames[0].toStdString(); + const auto topic = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/wrench"); + if (topic.empty()) + { + gzerr << "Unable to create publisher" << std::endl; + return; + } + this->dataPtr->pub = + this->dataPtr->node.Advertise(topic); + gzdbg << "Created publisher to " << topic << std::endl; + } + else + { + gzerr << "Unable to find world" << std::endl; + return; + } + + // Read configuration + if (_pluginElem) + { + if (auto elem = _pluginElem->FirstChildElement("rotation_stiffness")) + { + elem->QueryDoubleText(&this->dataPtr->rotStiffness); + emit this->RotStiffnessChanged(); + } + + if (auto elem = _pluginElem->FirstChildElement("position_stiffness")) + { + elem->QueryDoubleText(&this->dataPtr->posStiffness); + emit this->PosStiffnessChanged(); + } + } + + gz::gui::App()->findChild + ()->installEventFilter(this); +} + +///////////////////////////////////////////////// +bool MouseDrag::eventFilter(QObject *_obj, QEvent *_event) +{ + if (_event->type() == gz::gui::events::Render::kType) + { + this->dataPtr->OnRender(); + } + else if (_event->type() == gz::gui::events::LeftClickOnScene::kType) + { + // Mutex can't be locked on the whole eventFilter because that causes + // the program to freeze, since the Update function sends GUI events + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::RightClickOnScene::kType) + { + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::MousePressOnScene::kType) + { + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + else if (_event->type() == gz::gui::events::DragOnScene::kType) + { + std::lock_guard lock(this->dataPtr->mutex); + const auto event = + static_cast(_event); + this->dataPtr->mouseEvent = event->Mouse(); + this->dataPtr->mouseDirty = true; + } + + this->dataPtr->HandleMouseEvents(); + + return QObject::eventFilter(_obj, _event); +} + +///////////////////////////////////////////////// +void MouseDrag::Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("MouseDrag::Update"); + // Load the ApplyLinkWrench system + if (!this->dataPtr->systemLoaded) + { + const std::string name{"gz::sim::systems::ApplyLinkWrench"}; + const std::string filename{"gz-sim-apply-link-wrench-system"}; + const std::string innerxml{"0"}; + + // Get world entity + Entity worldEntity; + _ecm.Each( + [&](const Entity &_entity, + const components::World */*_world*/, + const components::Name *_name)->bool + { + if (_name->Data() == this->dataPtr->worldName) + { + worldEntity = _entity; + return false; + } + return true; + }); + + // Check if already loaded + const auto msg = + _ecm.ComponentData(worldEntity); + if (!msg) + { + gzdbg << "Unable to find SystemPluginInfo component for entity " + << worldEntity << std::endl; + return; + } + for (const auto &plugin : msg->plugins()) + { + if (plugin.filename() == filename) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system already loaded" << std::endl; + break; + } + } + + // Request to load system + if (!this->dataPtr->systemLoaded) + { + msgs::EntityPlugin_V req; + req.mutable_entity()->set_id(worldEntity); + auto plugin = req.add_plugins(); + plugin->set_name(name); + plugin->set_filename(filename); + plugin->set_innerxml(innerxml); + + msgs::Boolean res; + bool result; + const unsigned int timeout = 5000; + const auto service = transport::TopicUtils::AsValidTopic( + "/world/" + this->dataPtr->worldName + "/entity/system/add"); + if (service.empty()) + { + gzerr << "Unable to request " << service << std::endl; + return; + } + if (this->dataPtr->node.Request(service, req, timeout, res, result)) + { + this->dataPtr->systemLoaded = true; + gzdbg << "ApplyLinkWrench system has been loaded" << std::endl; + } + else + { + gzerr << "Error adding new system to entity: " + << worldEntity << "\n" + << "Name: " << name << "\n" + << "Filename: " << filename << "\n" + << "Inner XML: " << innerxml << std::endl; + return; + } + } + } + + std::lock_guard lock(this->dataPtr->mutex); + + if (this->dataPtr->mode == MouseDragMode::NONE || + _info.paused) + { + this->dataPtr->mode = MouseDragMode::NONE; + this->dataPtr->dragActive = false; + return; + } + + // Get Link corresponding to clicked Visual + const Link link(this->dataPtr->linkId); + const auto model = link.ParentModel(_ecm); + const auto linkWorldPose = worldPose(this->dataPtr->linkId, _ecm); + const auto inertial = + _ecm.Component(this->dataPtr->linkId); + if (!link.Valid(_ecm) || !inertial || + !model->Valid(_ecm) || model->Static(_ecm)) + { + this->dataPtr->blockOrbit = false; + return; + } + + if (this->dataPtr->blockOrbit) + this->dataPtr->sendBlockOrbit = true; + + this->dataPtr->dragActive = true; + + if (this->dataPtr->modeDirty) + { + this->dataPtr->modeDirty = false; + + gui::events::DeselectAllEntities event(true); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &event); + + this->dataPtr->mousePressPos = this->dataPtr->mouseEvent.Pos(); + this->dataPtr->initialRot = linkWorldPose.Rot(); + this->dataPtr->poseLast = linkWorldPose; + this->dataPtr->UpdateGains(inertial->Data()); + + // Calculate offset of force application from link origin + if (this->dataPtr->applyCOM || this->dataPtr->mode == MouseDragMode::ROTATE) + { + this->dataPtr->offset = inertial->Data().Pose().Pos(); + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + else + { + this->dataPtr->offset = linkWorldPose.Rot().RotateVectorReverse( + this->dataPtr->applicationPoint - linkWorldPose.Pos()); + } + + // The plane of wrench application should be normal to the center + // of the camera view and pass through the application point + const math::Vector3d direction = + this->dataPtr->camera->WorldPose().Rot().XAxis(); + const double planeOffset = + direction.Dot(this->dataPtr->applicationPoint); + this->dataPtr->plane = math::Planed(direction, planeOffset); + } + // Track the application point + else + { + this->dataPtr->applicationPoint = + linkWorldPose.Pos() + + linkWorldPose.Rot().RotateVector(this->dataPtr->offset); + } + + // Make a ray query at the mouse position + this->dataPtr->SetRayFromCamera(this->dataPtr->mouseEvent.Pos()); + const math::Vector3d end = this->dataPtr->rayQuery->Direction(); + + // Wrench in world coordinates, applied at the link origin + msgs::EntityWrench msg; + if (this->dataPtr->mode == MouseDragMode::ROTATE) + { + // Calculate rotation angle from mouse displacement + this->dataPtr->SetRayFromCamera(this->dataPtr->mousePressPos); + const math::Vector3d start = this->dataPtr->rayQuery->Direction(); + math::Vector3d axis = start.Cross(end); + const double angle = -atan2(axis.Length(), start.Dot(end)); + + // Project rotation axis onto plane + axis -= axis.Dot(this->dataPtr->plane.Normal()) * + this->dataPtr->plane.Normal(); + axis.Normalize(); + + // Calculate the necessary rotation and torque + this->dataPtr->goalRot = + math::Quaterniond(axis, angle) * this->dataPtr->initialRot; + const math::Quaterniond errorRot = + linkWorldPose.Rot() * this->dataPtr->goalRot.Inverse(); + msg = this->dataPtr->CalculateWrench( + errorRot.Euler(), _info.dt, linkWorldPose); + } + else if (this->dataPtr->mode == MouseDragMode::TRANSLATE) + { + const math::Vector3d origin = this->dataPtr->rayQuery->Origin(); + if (const auto t = this->dataPtr->plane.Intersection(origin, end)) + this->dataPtr->target = *t; + else + return; + + const math::Vector3d errorPos = + this->dataPtr->applicationPoint - this->dataPtr->target; + msg = this->dataPtr->CalculateWrench(errorPos, _info.dt, linkWorldPose); + } + + // Publish wrench + this->dataPtr->pub.Publish(msg); +} + +///////////////////////////////////////////////// +void MouseDrag::OnSwitchCOM(const bool _checked) +{ + this->dataPtr->applyCOM = _checked; +} + +///////////////////////////////////////////////// +double MouseDrag::RotStiffness() const +{ + return this->dataPtr->rotStiffness; +} + +///////////////////////////////////////////////// +void MouseDrag::SetRotStiffness(double _rotStiffness) +{ + this->dataPtr->rotStiffness = _rotStiffness; +} + +///////////////////////////////////////////////// +double MouseDrag::PosStiffness() const +{ + return this->dataPtr->posStiffness; +} + +///////////////////////////////////////////////// +void MouseDrag::SetPosStiffness(double _posStiffness) +{ + this->dataPtr->posStiffness = _posStiffness; +} + +///////////////////////////////////////////////// +void MouseDragPrivate::OnRender() +{ + std::lock_guard lock(this->mutex); + + // Get scene and user camera + if (!this->scene) + { + this->scene = rendering::sceneFromFirstRenderEngine(); + if (!this->scene) + { + return; + } + + for (unsigned int i = 0; i < this->scene->NodeCount(); ++i) + { + const auto cam = std::dynamic_pointer_cast( + this->scene->NodeByIndex(i)); + if (cam && cam->HasUserData("user-camera") && + std::get(cam->UserData("user-camera"))) + { + this->camera = cam; + gzdbg << "MouseDrag plugin is using camera [" + << this->camera->Name() << "]" << std::endl; + break; + } + } + + if (!this->camera) + { + gzerr << "MouseDrag camera is not available" << std::endl; + return; + } + this->rayQuery = this->scene->CreateRayQuery(); + + auto mat = this->scene->Material("Default/TransRed")->Clone(); + mat->SetDepthCheckEnabled(false); + mat->SetDepthWriteEnabled(false); + + this->arrowVisual = this->scene->CreateArrowVisual(); + this->arrowVisual->SetMaterial(mat); + this->arrowVisual->ShowArrowHead(true); + this->arrowVisual->ShowArrowShaft(true); + this->arrowVisual->ShowArrowRotation(false); + + this->boxVisual = scene->CreateVisual(); + this->boxVisual->AddGeometry(scene->CreateBox()); + this->boxVisual->SetInheritScale(false); + this->boxVisual->SetMaterial(mat); + this->boxVisual->SetUserData("gui-only", true); + } + + // Update the visualization + if (!this->dragActive) + { + this->arrowVisual->SetVisible(false); + this->boxVisual->SetVisible(false); + } + else if (this->mode == MouseDragMode::ROTATE) + { + this->boxVisual->SetLocalPosition(this->applicationPoint); + this->boxVisual->SetLocalRotation(this->goalRot); + this->boxVisual->SetLocalScale(1.2 * this->bboxSize); + + this->arrowVisual->SetVisible(false); + this->boxVisual->SetVisible(true); + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + const math::Vector3d axisDir = this->target - this->applicationPoint; + math::Quaterniond quat; + quat.SetFrom2Axes(math::Vector3d::UnitZ, axisDir); + const double scale = 2 * this->bboxSize.Length(); + this->arrowVisual->SetLocalPosition(this->applicationPoint); + this->arrowVisual->SetLocalRotation(quat); + this->arrowVisual->SetLocalScale(scale, scale, axisDir.Length() / 0.75); + + this->arrowVisual->SetVisible(true); + this->boxVisual->SetVisible(false); + } + + if (this->sendBlockOrbit) + { + // Events with false should only be sent once + if (!this->blockOrbit) + this->sendBlockOrbit = false; + + gz::gui::events::BlockOrbit blockOrbitEvent(this->blockOrbit); + gz::gui::App()->sendEvent( + gz::gui::App()->findChild(), + &blockOrbitEvent); + } +} + +///////////////////////////////////////////////// +void MouseDragPrivate::HandleMouseEvents() +{ + // Check for mouse events + if (!this->mouseDirty) + { + return; + } + this->mouseDirty = false; + + std::lock_guard lock(this->mutex); + + if (this->mouseEvent.Type() == common::MouseEvent::PRESS && + this->mouseEvent.Control() && + this->mouseEvent.Button() != common::MouseEvent::MIDDLE) + { + // Get the visual at mouse position + const rendering::VisualPtr visual = this->scene->VisualAt( + this->camera, + this->mouseEvent.Pos()); + + if (!visual || !visual->Parent()) + { + this->mode = MouseDragMode::NONE; + return; + } + try + { + this->linkId = + std::get(visual->Parent()->UserData("gazebo-entity")); + } + catch(std::bad_variant_access &e) + { + this->mode = MouseDragMode::NONE; + return; + } + + this->blockOrbit = true; + this->modeDirty = true; + + // Get the 3D coordinates of the clicked point + this->applicationPoint = rendering::screenToScene( + this->mouseEvent.Pos(), this->camera, + this->rayQuery); + + this->bboxSize = visual->LocalBoundingBox().Size(); + + if (this->mouseEvent.Button() == common::MouseEvent::LEFT) + { + this->mode = MouseDragMode::ROTATE; + } + else if (this->mouseEvent.Button() == common::MouseEvent::RIGHT) + { + this->mode = MouseDragMode::TRANSLATE; + } + } + else if (this->mouseEvent.Type() == common::MouseEvent::RELEASE) + { + this->mode = MouseDragMode::NONE; + this->dragActive = false; + this->blockOrbit = false; + } + else if (this->mouseEvent.Type() == common::MouseEvent::MOVE) + { + if (this->mode != MouseDragMode::NONE) + { + this->blockOrbit = true; + } + } +} + +///////////////////////////////////////////////// +void MouseDragPrivate::UpdateGains(const math::Inertiald &_inertial) +{ + if (this->mode == MouseDragMode::ROTATE) + { + // TODO(anyone): is this the best way to scale rotation gains? + const double avgInertia = + _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + this->pGainRot = this->rotStiffness * avgInertia; + this->dGainRot = 2 * sqrt(this->rotStiffness) * avgInertia; + this->pGainPos = 0.0; + this->dGainPos = 0.0; + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + const double mass = + _inertial.MassMatrix().Mass(); + this->pGainPos = this->posStiffness * mass; + this->dGainPos = 0.5 * sqrt(this->posStiffness) * mass; + this->pGainRot = 0.0; + + if (!this->applyCOM) + { + const double avgInertia = + _inertial.MassMatrix().PrincipalMoments().Sum() / 3; + this->dGainRot = 0.5 * sqrt(this->rotStiffness) * avgInertia; + } + } +} + +///////////////////////////////////////////////// +msgs::EntityWrench MouseDragPrivate::CalculateWrench( + const math::Vector3d &_error, + const std::chrono::duration &_dt, + const math::Pose3d &_linkWorldPose) +{ + math::Vector3d force, torque; + if (this->mode == MouseDragMode::ROTATE) + { + math::Vector3d dErrorRot = + (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() + / _dt.count(); + for (auto i : {0, 1, 2}) + { + dErrorRot[i] = this->CorrectAngle(dErrorRot[i]); + } + torque = - this->pGainRot * _error - this->dGainRot * dErrorRot; + } + else if (this->mode == MouseDragMode::TRANSLATE) + { + const math::Vector3d dErrorPos = + (_linkWorldPose.Pos() - this->poseLast.Pos()) / _dt.count(); + + force = - this->pGainPos * _error - this->dGainPos * dErrorPos; + torque = + _linkWorldPose.Rot().RotateVector(this->offset).Cross(force); + + // If the force is not applied to the center of mass, slightly damp the + // resulting rotation + if (!this->applyCOM) + { + math::Vector3d dErrorRot = + (_linkWorldPose.Rot() * this->poseLast.Rot().Inverse()).Euler() + / _dt.count(); + for (auto i : {0, 1, 2}) + { + dErrorRot[i] = this->CorrectAngle(dErrorRot[i]); + } + torque -= this->dGainRot * dErrorRot; + } + } + else + { + return msgs::EntityWrench(); + } + + this->poseLast = _linkWorldPose; + + msgs::EntityWrench msg; + msg.mutable_entity()->set_id(this->linkId); + msgs::Set(msg.mutable_wrench()->mutable_force(), force); + msgs::Set(msg.mutable_wrench()->mutable_torque(), torque); + return msg; +} + +///////////////////////////////////////////////// +void MouseDragPrivate::SetRayFromCamera(const math::Vector2i &_pos) +{ + // Normalize position on the image + const double width = this->camera->ImageWidth(); + const double height = this->camera->ImageHeight(); + + const double nx = 2.0 * _pos.X() / width - 1.0; + const double ny = 1.0 - 2.0 * _pos.Y() / height; + + // Make a ray query at the given position + this->rayQuery->SetFromCamera(this->camera, math::Vector2d(nx, ny)); +} + + +double MouseDragPrivate::CorrectAngle(const double _angle) +{ + double result = _angle; + if (_angle > GZ_PI) + result -= 2 * GZ_PI; + else if (_angle < -GZ_PI) + result += 2 * GZ_PI; + return result; +} + +// Register this plugin +GZ_ADD_PLUGIN(MouseDrag, gz::gui::Plugin) diff --git a/src/gui/plugins/mouse_drag/MouseDrag.hh b/src/gui/plugins/mouse_drag/MouseDrag.hh new file mode 100644 index 0000000000..6f61a31974 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.hh @@ -0,0 +1,107 @@ +/* + * 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_GUI_MOUSEDRAG_HH +#define GZ_GUI_MOUSEDRAG_HH + +#include + +#include + +namespace gz +{ +namespace sim +{ + class MouseDragPrivate; + + /// \brief Translate and rotate links by dragging them with the mouse. + /// Automatically loads the ApplyLinkWrench system. + /// + /// ## Configuration + /// + /// * \ : Stiffness of rotation mode, defaults to 100 + /// * \ : Stiffness of translation mode, defaults to 100 + class MouseDrag : public gz::sim::GuiSystem + { + Q_OBJECT + + /// \brief Stiffness of rotation mode + Q_PROPERTY( + double rotStiffness + READ RotStiffness + WRITE SetRotStiffness + NOTIFY RotStiffnessChanged + ) + + /// \brief Stiffness of translation mode + Q_PROPERTY( + double posStiffness + READ PosStiffness + WRITE SetPosStiffness + NOTIFY PosStiffnessChanged + ) + + /// \brief Constructor + public: MouseDrag(); + + /// \brief Destructor + public: ~MouseDrag() override; + + // Documentation inherited + public: void LoadConfig(const tinyxml2::XMLElement *_pluginElem) override; + + // Documentation inherited + protected: bool eventFilter(QObject *_obj, QEvent *_event) override; + + // Documentation inherited + public: void Update(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + /// \brief Callback when COM switch is pressed + /// \param[in] _checked True if force should be applied to center of mass + public slots: void OnSwitchCOM(const bool _checked); + + /// \brief Get the rotational stiffness + /// \return The rotational stiffness + public: Q_INVOKABLE double RotStiffness() const; + + /// \brief Notify that the rotational stiffness changed + signals: void RotStiffnessChanged(); + + /// \brief Set the rotational stiffness + /// \param[in] _rotStiffness The new rotational stiffness + public: Q_INVOKABLE void SetRotStiffness(double _rotStiffness); + + /// \brief Get the translational stiffness + /// \return The translational stiffness + public: Q_INVOKABLE double PosStiffness() const; + + /// \brief Notify that the translational stiffness changed + signals: void PosStiffnessChanged(); + + /// \brief Set the translational stiffness + /// \param[in] _posStiffness The new translational stiffness + public: Q_INVOKABLE void SetPosStiffness(double _posStiffness); + + /// \internal + /// \brief Pointer to private data. + private: std::unique_ptr dataPtr; + }; +} +} + +#endif diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qml b/src/gui/plugins/mouse_drag/MouseDrag.qml new file mode 100644 index 0000000000..505649ad01 --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.qml @@ -0,0 +1,98 @@ +/* + * 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. + * +*/ +import QtQuick 2.9 +import QtQuick.Controls 2.1 +import QtQuick.Layouts 1.3 +import "qrc:/qml" + +GridLayout { + columns: 8 + columnSpacing: 10 + Layout.minimumWidth: 350 + Layout.minimumHeight: 300 + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + + // Maximum value for GzSpinBoxes + property int maxValue: 1000000 + + // Precision of the GzSpinBoxes in decimal places + property int decimalPlaces: 1 + + // Step size of the GzSpinBoxes + property double step: 10.0 + + Text { + Layout.columnSpan: 8 + id: rotationText + text: qsTr("Rotation (Ctrl+Left-Click)") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: rotStiffText + text: qsTr("Rotation stiffness") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: rotStiffness + maximumValue: maxValue + minimumValue: 0 + value: MouseDrag.rotStiffness + decimals: decimalPlaces + stepSize: step + onValueChanged: MouseDrag.rotStiffness = rotStiffness.value + } + + Text { + Layout.columnSpan: 8 + id: translationText + text: qsTr("Translation (Ctrl+Right-Click)") + } + + Label { + Layout.columnSpan: 2 + horizontalAlignment: Text.AlignRight + id: posStiffText + text: qsTr("Position stiffness") + } + + GzSpinBox { + Layout.columnSpan: 6 + Layout.fillWidth: true + id: posStiffness + maximumValue: maxValue + minimumValue: 0 + value: MouseDrag.posStiffness + decimals: decimalPlaces + stepSize: step + onValueChanged: MouseDrag.posStiffness = posStiffness.value + } + + Switch { + Layout.columnSpan: 8 + objectName: "switchCOM" + text: qsTr("Apply force to center of mass") + onToggled: { + MouseDrag.OnSwitchCOM(checked); + } + } +} diff --git a/src/gui/plugins/mouse_drag/MouseDrag.qrc b/src/gui/plugins/mouse_drag/MouseDrag.qrc new file mode 100644 index 0000000000..89e46ce6cb --- /dev/null +++ b/src/gui/plugins/mouse_drag/MouseDrag.qrc @@ -0,0 +1,5 @@ + + + MouseDrag.qml + + diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index e268d5f05b..7e61cc0c4e 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -23,6 +23,7 @@ #include #include +#include #include #include @@ -42,6 +43,9 @@ #include "gz/sim/EntityComponentManager.hh" + +Q_DECLARE_METATYPE(gz::sim::Resource) + namespace gz::sim { class ResourceSpawnerPrivate @@ -72,9 +76,34 @@ namespace gz::sim /// \brief Holds all of the relevant data used by `DisplayData()` in order /// to filter and sort the displayed resources as desired by the user. public: Display displayData; + + /// \brief The list of Fuel servers to download from. + public: std::vector servers; + + /// \brief Data structure to hold relevant bits for a worker thread that + /// fetches the list of recources available for an owner on Fuel. + struct FetchResourceListWorker + { + /// \brief Thread that runs the worker + std::thread thread; + /// \brief Flag to notify the worker that it needs to stop. This could be + /// when an owner is removed or when the program exits. + std::atomic stopDownloading{false}; + /// \brief The workers own Fuel client to avoid synchronization. + fuel_tools::FuelClient fuelClient; + }; + + /// \brief Holds a map from owner to the associated resource list worker. + public: std::unordered_map fetchResourceListWorkers; }; } +namespace { + +// Default owner to be fetched from Fuel. This owner cannot be removed. +constexpr const char *kDefaultOwner = "openrobotics"; +} using namespace gz; using namespace sim; @@ -88,15 +117,27 @@ void PathModel::AddPath(const std::string &_path) { GZ_PROFILE_THREAD_NAME("Qt thread"); GZ_PROFILE("PathModel::AddPath"); - QStandardItem *parentItem{nullptr}; - - parentItem = this->invisibleRootItem(); - auto localModel = new QStandardItem(QString::fromStdString(_path)); localModel->setData(QString::fromStdString(_path), this->roleNames().key("path")); - parentItem->appendRow(localModel); + this->appendRow(localModel); +} + +///////////////////////////////////////////////// +void PathModel::RemovePath(const std::string &_path) +{ + GZ_PROFILE_THREAD_NAME("Qt thread"); + GZ_PROFILE("PathModel::RemovePath"); + QString qPath = QString::fromStdString(_path); + for (int i = 0; i < this->rowCount(); ++i) + { + if (this->data(this->index(i, 0)) == qPath) + { + this->removeRow(i); + break; + } + } } ///////////////////////////////////////////////// @@ -116,14 +157,9 @@ ResourceModel::ResourceModel() : QStandardItemModel() ///////////////////////////////////////////////// void ResourceModel::Clear() { - QStandardItem *parentItem{nullptr}; - parentItem = this->invisibleRootItem(); - - while (parentItem->rowCount() > 0) - { - parentItem->removeRow(0); - } + this->clear(); this->gridIndex = 0; + emit sizeChanged(); } ///////////////////////////////////////////////// @@ -134,13 +170,10 @@ void ResourceModel::AddResources(std::vector &_resources) } ///////////////////////////////////////////////// -void ResourceModel::AddResource(Resource &_resource) +void ResourceModel::AddResource(const Resource &_resource) { GZ_PROFILE_THREAD_NAME("Qt thread"); GZ_PROFILE("GridModel::AddResource"); - QStandardItem *parentItem{nullptr}; - - parentItem = this->invisibleRootItem(); auto resource = new QStandardItem(QString::fromStdString(_resource.name)); resource->setData(_resource.isFuel, @@ -168,8 +201,9 @@ void ResourceModel::AddResource(Resource &_resource) this->roleNames().key("index")); this->gridIndex++; } + emit sizeChanged(); - parentItem->appendRow(resource); + this->appendRow(resource); } ///////////////////////////////////////////////// @@ -211,6 +245,7 @@ ResourceSpawner::ResourceSpawner() : gz::gui::Plugin(), dataPtr(std::make_unique()) { + qRegisterMetaType(); gz::gui::App()->Engine()->rootContext()->setContextProperty( "ResourceList", &this->dataPtr->resourceModel); gz::gui::App()->Engine()->rootContext()->setContextProperty( @@ -219,10 +254,45 @@ ResourceSpawner::ResourceSpawner() "OwnerList", &this->dataPtr->ownerModel); this->dataPtr->fuelClient = std::make_unique(); + + 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); + } + } + + this->dataPtr->servers = servers; } ///////////////////////////////////////////////// -ResourceSpawner::~ResourceSpawner() = default; +ResourceSpawner::~ResourceSpawner() +{ + for (auto &workers : this->dataPtr->fetchResourceListWorkers) + { + workers.second.stopDownloading = true; + if (workers.second.thread.joinable()) + { + workers.second.thread.join(); + } + } +} ///////////////////////////////////////////////// void ResourceSpawner::SetThumbnail(const std::string &_thumbnailPath, @@ -332,7 +402,7 @@ std::vector ResourceSpawner::FuelResources(const std::string &_owner) if (this->dataPtr->ownerModelMap.find(_owner) != this->dataPtr->ownerModelMap.end()) { - for (Resource resource : this->dataPtr->ownerModelMap[_owner]) + for (const Resource &resource : this->dataPtr->ownerModelMap[_owner]) { fuelResources.push_back(resource); } @@ -551,85 +621,9 @@ void ResourceSpawner::LoadConfig(const tinyxml2::XMLElement *) this->AddPath(path); } - 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 - this->dataPtr->ownerModel.AddPath("Please wait... Loading models from Fuel."); - - // Pull in fuel models asynchronously - std::thread t([this, servers] - { - // A set isn't necessary to keep track of the owners, but it - // maintains alphabetical order - std::set ownerSet; - for (auto const &server : servers) - { - std::vector models; - for (auto iter = this->dataPtr->fuelClient->Models(server); iter; ++iter) - { - models.push_back(iter->Identification()); - } - - // Create each fuel resource and add them to the ownerModelMap - for (const auto &id : models) - { - Resource resource; - resource.name = id.Name(); - resource.isFuel = true; - resource.isDownloaded = false; - resource.owner = id.Owner(); - resource.sdfPath = id.UniqueName(); - std::string path; - - // If the resource is cached, we can go ahead and populate the - // respective information - if (this->dataPtr->fuelClient->CachedModel( - common::URI(id.UniqueName()), path)) - { - resource.isDownloaded = true; - resource.sdfPath = common::joinPaths(path, "model.sdf"); - std::string thumbnailPath = common::joinPaths(path, "thumbnails"); - this->SetThumbnail(thumbnailPath, resource); - } - ownerSet.insert(id.Owner()); - this->dataPtr->ownerModelMap[id.Owner()].push_back(resource); - } - } - - // Clear the loading message - this->dataPtr->ownerModel.clear(); - - // Add all unique owners to the owner model - for (const auto &resource : ownerSet) - { - this->dataPtr->ownerModel.AddPath(resource); - } - gzmsg << "Fuel resources loaded.\n"; - }); - t.detach(); + this->dataPtr->ownerModel.AddPath(kDefaultOwner); + RunFetchResourceListThread(kDefaultOwner); } ///////////////////////////////////////////////// @@ -655,6 +649,112 @@ void ResourceSpawner::OnResourceSpawn(const QString &_sdfPath) &event); } +///////////////////////////////////////////////// +void ResourceSpawner::UpdateOwnerListModel(Resource _resource) +{ + // If the resource is cached, we can go ahead and populate the + // respective information + std::string path; + if (this->dataPtr->fuelClient->CachedModel( + common::URI(_resource.sdfPath), path)) + { + _resource.isDownloaded = true; + _resource.sdfPath = common::joinPaths(path, "model.sdf"); + std::string thumbnailPath = common::joinPaths(path, "thumbnails"); + this->SetThumbnail(thumbnailPath, _resource); + } + + this->dataPtr->ownerModelMap[_resource.owner].push_back(_resource); + if (this->dataPtr->displayData.ownerPath == _resource.owner) + { + this->dataPtr->resourceModel.AddResource(_resource); + } +} + +///////////////////////////////////////////////// +bool ResourceSpawner::AddOwner(const QString &_owner) +{ + const std::string ownerString = _owner.toStdString(); + if (this->dataPtr->ownerModelMap.find(ownerString) != + this->dataPtr->ownerModelMap.end()) + { + QString errorMsg = QString("Owner %1 already added").arg(_owner); + emit resourceSpawnerError(errorMsg); + return false; + } + this->dataPtr->ownerModel.AddPath(ownerString); + RunFetchResourceListThread(ownerString); + return true; +} + +///////////////////////////////////////////////// +void ResourceSpawner::RemoveOwner(const QString &_owner) +{ + const std::string ownerString = _owner.toStdString(); + this->dataPtr->ownerModelMap.erase(ownerString); + this->dataPtr->ownerModel.RemovePath(ownerString); + this->dataPtr->fetchResourceListWorkers[ownerString].stopDownloading = true; +} + +///////////////////////////////////////////////// +bool ResourceSpawner::IsDefaultOwner(const QString &_owner) const +{ + return _owner.toStdString() == kDefaultOwner; +} + +///////////////////////////////////////////////// +void ResourceSpawner::RunFetchResourceListThread(const std::string &_owner) +{ + auto &worker = this->dataPtr->fetchResourceListWorkers[_owner]; + // If the owner had been deleted, we need to clean the previous thread and + // restart. + if (worker.thread.joinable()) + { + worker.stopDownloading = true; + worker.thread.join(); + } + + worker.stopDownloading = false; + + // Pull in fuel models asynchronously + this->dataPtr->fetchResourceListWorkers[_owner].thread = std::thread( + [this, owner = _owner, &worker] + { + int counter = 0; + for (auto const &server : this->dataPtr->servers) + { + fuel_tools::ModelIdentifier modelId; + modelId.SetServer(server); + modelId.SetOwner(owner); + for (auto iter = worker.fuelClient.Models(modelId, false); + iter; ++iter, ++counter) + { + if (worker.stopDownloading) + { + return; + } + auto id = iter->Identification(); + Resource resource; + resource.name = id.Name(); + resource.isFuel = true; + resource.isDownloaded = false; + resource.owner = id.Owner(); + resource.sdfPath = id.UniqueName(); + + QMetaObject::invokeMethod( + this, "UpdateOwnerListModel", Qt::QueuedConnection, + Q_ARG(gz::sim::Resource, resource)); + } + } + if (counter == 0) + { + QString errorMsg = QString("No resources found for %1") + .arg(QString::fromStdString(owner)); + emit resourceSpawnerError(errorMsg); + } + }); +} + // Register this plugin GZ_ADD_PLUGIN(ResourceSpawner, gz::gui::Plugin) diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.hh b/src/gui/plugins/resource_spawner/ResourceSpawner.hh index 879545a943..4e704f69a9 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.hh +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.hh @@ -18,10 +18,13 @@ #ifndef GZ_SIM_GUI_RESOURCE_SPAWNER_HH_ #define GZ_SIM_GUI_RESOURCE_SPAWNER_HH_ +#include #include #include #include #include +#include +#include #include @@ -93,9 +96,13 @@ namespace sim /// \brief Destructor public: ~PathModel() override = default; - /// \brief Add a local model to the grid view. - /// param[in] _model The local model to be added - public slots: void AddPath(const std::string &_path); + /// \brief Add a path. + /// param[in] _path The path to be added. + public: void AddPath(const std::string &_path); + + /// \brief Remove a path. + /// param[in] _path The path to be removed. + public: void RemovePath(const std::string &_path); // Documentation inherited public: QHash roleNames() const override; @@ -107,6 +114,10 @@ namespace sim { Q_OBJECT + /// \brief Property used to display the total number of resources associated + /// with an owner. + Q_PROPERTY(int totalCount MEMBER gridIndex NOTIFY sizeChanged) + /// \brief Constructor public: explicit ResourceModel(); @@ -115,7 +126,7 @@ namespace sim /// \brief Add a resource to the grid view. /// param[in] _resource The local resource to be added - public: void AddResource(Resource &_resource); + public: void AddResource(const Resource &_resource); /// \brief Add a vector of resources to the grid view. /// param[in] _resource The vector of local resources to be added @@ -134,6 +145,9 @@ namespace sim // Documentation inherited public: QHash roleNames() const override; + /// \brief Signal used with the totalCount property + public: signals: void sizeChanged(); + // \brief Index to keep track of the position of each resource in the qml // grid, used primarily to access currently loaded resources for updates. public: int gridIndex = 0; @@ -238,6 +252,36 @@ namespace sim public: void SetThumbnail(const std::string &_thumbnailPath, Resource &_resource); + /// \brief Called form a download thread to update the GUI's list of + /// resources. + /// \param[in] _resource The resource fetched from Fuel. Note that it is + /// passed by value as a copy is necessary to update the resource if it's + /// cached. + public: Q_INVOKABLE void UpdateOwnerListModel(gz::sim::Resource _resource); + + /// \brief Add owner to the list of owners whose resources would be fetched + /// from Fuel. + /// \param[in] _owner Name of owner. + /// \return True if the owner was successfully added. + public: Q_INVOKABLE bool AddOwner(const QString &_owner); + + /// \brief Remove owner from the list of owners whose resources would be + /// fetched from Fuel. + /// \param[in] _owner Name of owner. + public: Q_INVOKABLE void RemoveOwner(const QString &_owner); + + /// \brief Determine if owner is the default owner + /// \param[in] _owner Name of owner. + public: Q_INVOKABLE bool IsDefaultOwner(const QString &_owner) const; + + /// \brief Signal emitted when an error is encountered regarding an owner + /// \param[in] _errorMsg Error message to be displayed. + signals: void resourceSpawnerError(const QString &_errorMsg); + + /// \brief Starts a thread that fetches the resources list for a given owner + /// \param[in] _owner Name of owner. + private: void RunFetchResourceListThread(const std::string &_owner); + /// \internal /// \brief Pointer to private data. private: std::unique_ptr dataPtr; diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.qml b/src/gui/plugins/resource_spawner/ResourceSpawner.qml index 51ac1a37e2..3ca3921ab5 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.qml +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.qml @@ -98,14 +98,15 @@ Rectangle { border.color: "gray" border.width: 1 Layout.alignment: Qt.AlignLeft - Layout.preferredHeight: 25 + Layout.preferredHeight: 35 Layout.fillWidth: true + Layout.leftMargin: -border.width + Layout.rightMargin: -border.width Label { - topPadding: 2 - leftPadding: 5 + padding: 5 text: "Local resources" anchors.fill: parent - font.pointSize: 12 + font.pointSize: 14 } } TreeView { @@ -121,6 +122,7 @@ Rectangle { verticalScrollBarPolicy: Qt.ScrollBarAsNeeded headerVisible: false backgroundVisible: false + frameVisible: false headerDelegate: Rectangle { visible: false @@ -143,7 +145,7 @@ Rectangle { height: treeItemHeight } itemDelegate: Rectangle { - id: item + id: localItem color: styleData.selected ? Material.accent : (styleData.row % 2 == 0) ? evenColor : oddColor height: treeItemHeight @@ -188,7 +190,7 @@ Rectangle { ToolTip { visible: ma.containsMouse delay: 500 - y: item.z - 30 + y: localItem.z - 30 text: model === null ? "?" : model.path enter: null @@ -207,100 +209,136 @@ Rectangle { color: evenColor border.color: "gray" Layout.alignment: Qt.AlignLeft - Layout.preferredHeight: 25 + Layout.preferredHeight: 35 Layout.fillWidth: true + border.width: 1 + Layout.leftMargin: -border.width + Layout.rightMargin: -border.width + Layout.topMargin: -border.width Label { text: "Fuel resources" - topPadding: 2 - leftPadding: 5 + padding: 5 anchors.fill: parent - font.pointSize: 12 + font.pointSize: 14 } } - TreeView { - id: treeView2 + + ListView { + id: listView model: OwnerList - // For some reason, SingleSelection is not working - selectionMode: SelectionMode.MultiSelection - verticalScrollBarPolicy: Qt.ScrollBarAsNeeded - headerVisible: false - backgroundVisible: false - Layout.minimumWidth: 300 - Layout.alignment: Qt.AlignCenter Layout.fillWidth: true Layout.fillHeight: true + Layout.minimumWidth: 300 + clip: true - headerDelegate: Rectangle { - visible: false + ScrollBar.vertical: ScrollBar { + active: true; } - TableViewColumn - { - role: "name" - } + delegate: Rectangle { + id: fuelItem2 + color: ListView.view.currentIndex == index ? Material.accent : (index % 2 == 0) ? evenColor : oddColor + height: treeItemHeight + width: ListView.view.width + ListView.onAdd : { + ListView.view.currentIndex = index + } - selection: ItemSelectionModel { - model: OwnerList - } + ListView.onCurrentItemChanged: { + if (index >= 0) { + currentPath = model.path + ResourceSpawner.OnOwnerClicked(model.path) + ResourceSpawner.DisplayResources(); + treeView.selection.clearSelection() + gridView.currentIndex = -1 + } + } - style: TreeViewStyle { - indentation: 0 - rowDelegate: Rectangle { - id: row2 - color: Material.background - height: treeItemHeight + MouseArea { + anchors.fill: parent + onClicked: { + listView.currentIndex = index + } } - itemDelegate: Rectangle { - id: item - color: styleData.selected ? Material.accent : (styleData.row % 2 == 0) ? evenColor : oddColor - height: treeItemHeight - anchors.top: parent.top - anchors.right: parent.right + RowLayout { + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + clip: true Image { - id: dirIcon - source: styleData.selected ? "folder_open.png" : "folder_closed.png" - height: treeItemHeight * 0.6 - width: treeItemHeight * 0.6 - anchors.verticalCenter: parent.verticalCenter - anchors.left: parent.left + id: dirIcon2 + source: listView.currentIndex == index ? "folder_open.png" : "folder_closed.png" + Layout.preferredHeight: treeItemHeight * 0.6 + Layout.preferredWidth: treeItemHeight * 0.6 } Label { - text: (model === null) ? "" : model.path + text: model.path + Layout.fillWidth: true elide: Text.ElideMiddle font.pointSize: 12 - anchors.leftMargin: 1 - anchors.left: dirIcon.right - anchors.verticalCenter: parent.verticalCenter leftPadding: 2 } - MouseArea { - id: ma - anchors.fill: parent - propagateComposedEvents: true - hoverEnabled: true + Button { + // unicode for emdash (—) + text: "\u2014" + flat: true + Layout.fillHeight : true + Layout.preferredWidth: 30 + visible: !ResourceSpawner.IsDefaultOwner(model.path) + onClicked: { - ResourceSpawner.OnOwnerClicked(model.path) - ResourceSpawner.DisplayResources(); - treeView2.selection.select(styleData.index, ItemSelectionModel.ClearAndSelect) - treeView.selection.clearSelection() - currentPath = model.path - gridView.currentIndex = -1 - mouse.accepted = false + ResourceSpawner.RemoveOwner(model.path) } } + } + } + } - ToolTip { - visible: ma.containsMouse - delay: 500 - y: item.z - 30 - text: model === null ? - "?" : model.path - enter: null - exit: null + // Add owner button + Rectangle { + id: addOwnerBar + color: evenColor + Layout.minimumHeight: 50 + Layout.fillWidth: true + clip:true + RowLayout { + anchors.fill: parent + anchors.leftMargin: 10 + anchors.rightMargin: 10 + spacing: 10 + + TextField { + Layout.fillWidth: true + id: ownerInput + selectByMouse: true + color: Material.theme == Material.Light ? "black" : "white" + placeholderText: "Add owner" + function processInput() { + if (text != "" && ResourceSpawner.AddOwner(text)) { + text = "" + } + } + onAccepted: { + processInput(); + } + } + + RoundButton { + Material.background: Material.Green + contentItem: Label { + text: "+" + color: "white" + font.pointSize: 30 + horizontalAlignment: Text.AlignHCenter + verticalAlignment: Text.AlignVCenter + } + padding: 0 + onClicked: { + ownerInput.processInput() } } } @@ -322,6 +360,7 @@ Rectangle { RowLayout { id: rowLayout spacing: 7 + anchors.fill: parent Rectangle { color: "transparent" height: 25 @@ -354,10 +393,11 @@ Rectangle { } Rectangle { color: "transparent" - height: 50 + implicitHeight: sortComboBox.implicitHeight Layout.minimumWidth: 140 Layout.preferredWidth: (searchSortBar.width - 80) / 2 ComboBox { + id: sortComboBox anchors.fill: parent model: ListModel { id: cbItems @@ -379,9 +419,9 @@ Rectangle { Layout.fillWidth: true Layout.minimumWidth: 300 height: 40 - color: "transparent" + color: Material.accent Label { - text: currentPath + text: currentPath ? "Owner: " + currentPath + " (" + gridView.model.totalCount + ")" : "" font.pointSize: 12 elide: Text.ElideMiddle anchors.margins: 5 @@ -420,6 +460,8 @@ Rectangle { layer.effect: ElevationEffect { elevation: 6 } + border.width: 1 + border.color: "lightgray" } ColumnLayout { @@ -438,7 +480,7 @@ Rectangle { Layout.margins: 1 source: (model.isFuel && !model.isDownloaded) ? "DownloadToUse.png" : - (model.thumbnail == "" ? + (model.thumbnail === "" ? "NoThumbnail.png" : "file:" + model.thumbnail) fillMode: Image.PreserveAspectFit } @@ -470,6 +512,7 @@ Rectangle { modal: true focus: true title: "Note" + standardButtons: Dialog.Ok Rectangle { color: "transparent" anchors.fill: parent @@ -518,4 +561,29 @@ Rectangle { } } } + + // Dialog for error messages + Dialog { + id: messageDialog + width: 360 + height: 150 + parent: resourceSpawner.Window.window ? resourceSpawner.Window.window.contentItem : resourceSpawner + x: Math.round((parent.width - width) / 2) + y: Math.round((parent.height - height) / 2) + modal: true + focus: true + title: "Error" + standardButtons: Dialog.Ok + contentItem: Text { + text: "" + } + } + + Connections { + target: ResourceSpawner + onResourceSpawnerError : { + messageDialog.contentItem.text = _errorMsg + messageDialog.visible = true + } + } } diff --git a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc index c62609d05b..82e1492853 100644 --- a/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc +++ b/src/gui/plugins/visualization_capabilities/VisualizationCapabilities.cc @@ -1498,6 +1498,13 @@ rendering::VisualPtr VisualizationCapabilitiesPrivate::CreateCollisionVisual( if (!_visual.Geom()) return rendering::VisualPtr(); + if (_visual.Geom()->Type() == sdf::GeometryType::HEIGHTMAP) + { + gzwarn << "Collision visualization for heightmaps are not supported yet." + << std::endl; + return rendering::VisualPtr(); + } + std::string name = _visual.Name().empty() ? std::to_string(_id) : _visual.Name(); if (_parent) diff --git a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc index c6caa20145..ec96e9d909 100644 --- a/src/gui/plugins/visualize_lidar/VisualizeLidar.cc +++ b/src/gui/plugins/visualize_lidar/VisualizeLidar.cc @@ -180,7 +180,7 @@ void VisualizeLidar::LoadLidar() return; } - if (!scene->IsInitialized() || scene->VisualCount() == 0) + if (!scene->IsInitialized()) { return; } diff --git a/src/gz.cc b/src/gz.cc index cd71e586bf..2d16b52789 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -452,3 +452,8 @@ extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, return gz::sim::gui::runGui( argc, argv, _guiConfig, _file, _waitGui, _renderEngine); } + +int main(int argc, char* argv[]) +{ + return sim::gui::runGui(argc, argv, nullptr); +} diff --git a/src/rendering/CMakeLists.txt b/src/rendering/CMakeLists.txt index a8a510694f..2b551f9da2 100644 --- a/src/rendering/CMakeLists.txt +++ b/src/rendering/CMakeLists.txt @@ -2,6 +2,7 @@ set (rendering_comp_sources MarkerManager.cc RenderUtil.cc SceneManager.cc + WrenchVisualizer.cc ) if (MSVC) diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 6c216ce5cf..50b761cecb 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -127,6 +127,9 @@ class gz::sim::MarkerManagerPrivate /// \brief Topic name for the marker service public: std::string topicName = "/marker"; + + /// \brief Topic that publishes marker updates. + public: gz::transport::Node::Publisher markerPub; }; ///////////////////////////////////////////////// @@ -197,6 +200,9 @@ bool MarkerManager::Init(const rendering::ScenePtr &_scene) << "_array service.\n"; } + this->dataPtr->markerPub = + this->dataPtr->node.Advertise(this->dataPtr->topicName); + return true; } @@ -223,6 +229,7 @@ void MarkerManagerPrivate::Update() markerIter != this->markerMsgs.end();) { this->ProcessMarkerMsg(*markerIter); + this->markerPub.Publish(*markerIter); this->markerMsgs.erase(markerIter++); } diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc index f1f758e8e9..5684d4b047 100644 --- a/src/rendering/SceneManager.cc +++ b/src/rendering/SceneManager.cc @@ -702,23 +702,22 @@ rendering::GeometryPtr SceneManager::LoadGeometry(const sdf::Geometry &_geom, } else if (_geom.Type() == sdf::GeometryType::MESH) { - auto fullPath = asFullPath(_geom.MeshShape()->Uri(), - _geom.MeshShape()->FilePath()); - if (fullPath.empty()) - { - gzerr << "Mesh geometry missing uri" << std::endl; - return geom; - } rendering::MeshDescriptor descriptor; - - // Assume absolute path to mesh file - descriptor.meshName = fullPath; + descriptor.mesh = loadMesh(*_geom.MeshShape()); + if (!descriptor.mesh) + return geom; + std::string meshUri = + (common::URI(_geom.MeshShape()->Uri()).Scheme() == "name") ? + common::basename(_geom.MeshShape()->Uri()) : + asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + auto a = asFullPath(_geom.MeshShape()->Uri(), + _geom.MeshShape()->FilePath()); + + descriptor.meshName = meshUri; descriptor.subMeshName = _geom.MeshShape()->Submesh(); descriptor.centerSubMesh = _geom.MeshShape()->CenterSubmesh(); - common::MeshManager *meshManager = - common::MeshManager::Instance(); - descriptor.mesh = meshManager->Load(descriptor.meshName); geom = this->dataPtr->scene->CreateMesh(descriptor); scale = _geom.MeshShape()->Scale(); } diff --git a/src/rendering/WrenchVisualizer.cc b/src/rendering/WrenchVisualizer.cc new file mode 100644 index 0000000000..61203f501f --- /dev/null +++ b/src/rendering/WrenchVisualizer.cc @@ -0,0 +1,130 @@ +/* + * 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 "gz/sim/rendering/WrenchVisualizer.hh" + +using namespace gz; +using namespace sim; +using namespace detail; + +/// Private data for the WrenchVisualizer class +class gz::sim::detail::WrenchVisualizer::Implementation +{ + /// \brief Pointer to the rendering scene + public: rendering::ScenePtr scene{nullptr}; +}; + +///////////////////////////////////////////////// +WrenchVisualizer::WrenchVisualizer() : + dataPtr(utils::MakeUniqueImpl()) +{ +} + +///////////////////////////////////////////////// +WrenchVisualizer::~WrenchVisualizer() = default; + +///////////////////////////////////////////////// +bool WrenchVisualizer::Init(rendering::ScenePtr _scene) +{ + if (!_scene) + { + return false; + } + this->dataPtr->scene = _scene; + return true; +} + +///////////////////////////////////////////////// +rendering::ArrowVisualPtr WrenchVisualizer::CreateForceVisual( + rendering::MaterialPtr _material) +{ + if (!this->dataPtr->scene) + { + gzwarn << "WrenchVisualizer not initialized" << std::endl; + return rendering::ArrowVisualPtr(); + } + rendering::ArrowVisualPtr forceVisual = + this->dataPtr->scene->CreateArrowVisual(); + forceVisual->SetMaterial(_material); + forceVisual->ShowArrowHead(true); + forceVisual->ShowArrowShaft(true); + forceVisual->ShowArrowRotation(false); + return forceVisual; +} + +///////////////////////////////////////////////// +rendering::VisualPtr WrenchVisualizer::CreateTorqueVisual( + rendering::MaterialPtr _material) +{ + if (!this->dataPtr->scene) + { + gzwarn << "WrenchVisualizer not initialized" << std::endl; + return rendering::ArrowVisualPtr(); + } + auto meshMgr = common::MeshManager::Instance(); + std::string meshName{"torque_tube"}; + if (!meshMgr->HasMesh(meshName)) + meshMgr->CreateTube(meshName, 0.28f, 0.3f, 0.2f, 1, 32); + auto torqueTube = this->dataPtr->scene->CreateVisual(); + torqueTube->AddGeometry(this->dataPtr->scene->CreateMesh(meshName)); + torqueTube->SetOrigin(0, 0, -0.9f); + torqueTube->SetLocalPosition(0, 0, 0); + + auto cylinder = this->dataPtr->scene->CreateVisual(); + cylinder->AddGeometry(this->dataPtr->scene->CreateCylinder()); + cylinder->SetOrigin(0, 0, -0.5); + cylinder->SetLocalScale(0.01, 0.01, 0.8); + cylinder->SetLocalPosition(0, 0, 0); + + auto torqueVisual = this->dataPtr->scene->CreateVisual(); + torqueVisual->AddChild(torqueTube); + torqueVisual->AddChild(cylinder); + torqueVisual->SetMaterial(_material); + return torqueVisual; +} + +///////////////////////////////////////////////// +void WrenchVisualizer::UpdateVectorVisual(rendering::VisualPtr _visual, + const math::Vector3d &_direction, + const math::Vector3d &_position, + const double _size, + const bool _tip) +{ + math::Quaterniond quat; + quat.SetFrom2Axes(math::Vector3d::UnitZ, _direction); + if (_tip) + { + _visual->SetLocalPosition( + _position - 0.75 * _size * _direction.Normalized()); + } + else + { + _visual->SetLocalPosition(_position); + } + _visual->SetLocalRotation(quat); + _visual->SetLocalScale(_size); +} diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 084d3b71cd..98740bb721 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -80,16 +80,19 @@ class gz::sim::systems::ApplyLinkWrenchPrivate /// \param[in] _msg Entity message. If it's a link, that link is returned. If /// it's a model, its canonical link is returned. /// \param[out] Force to apply. +/// \param[out] Offset of the force application point expressed in the link +/// frame. /// \param[out] Torque to apply. +/// \param[out] Offset of the force application point expressed in the link +/// frame. /// \return Target link entity. Link decomposeMessage(const EntityComponentManager &_ecm, const msgs::EntityWrench &_msg, math::Vector3d &_force, - math::Vector3d &_torque) + math::Vector3d &_torque, math::Vector3d &_offset) { if (_msg.wrench().has_force_offset()) { - gzwarn << "Force offset currently not supported, it will be ignored." - << std::endl; + _offset = msgs::Convert(_msg.wrench().force_offset()); } if (_msg.wrench().has_force()) @@ -148,8 +151,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, this->dataPtr->verbose = _sdf->Get("verbose", true).first; // Initial wrenches - auto ptr = const_cast(_sdf.get()); - for (auto elem = ptr->GetElement("persistent"); + for (auto elem = _sdf->FindElement("persistent"); elem != nullptr; elem = elem->GetNextElement("persistent")) { @@ -163,7 +165,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, msg.mutable_entity()->set_name(elem->Get("entity_name")); - auto typeStr = elem->GetElement("entity_type")->Get(); + auto typeStr = elem->FindElement("entity_type")->Get(); if (typeStr == "link") { msg.mutable_entity()->set_type(msgs::Entity::LINK); @@ -182,12 +184,12 @@ void ApplyLinkWrench::Configure(const Entity &_entity, if (elem->HasElement("force")) { msgs::Set(msg.mutable_wrench()->mutable_force(), - elem->GetElement("force")->Get()); + elem->FindElement("force")->Get()); } if (elem->HasElement("torque")) { msgs::Set(msg.mutable_wrench()->mutable_torque(), - elem->GetElement("torque")->Get()); + elem->FindElement("torque")->Get()); } this->dataPtr->OnWrenchPersistent(msg); } @@ -270,8 +272,9 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, auto msg = this->dataPtr->newWrenches.front(); math::Vector3d force; + math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, torque); + auto link = decomposeMessage(_ecm, msg, force, torque, offset); if (!link.Valid(_ecm)) { gzerr << "Entity not found." << std::endl @@ -280,11 +283,12 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, continue; } - link.AddWorldWrench(_ecm, force, torque); + link.AddWorldWrench(_ecm, force, torque, offset); if (this->dataPtr->verbose) { - gzdbg << "Applying wrench [" << force << " " << torque << "] to entity [" + gzdbg << "Applying wrench [" << force << " " << torque + << "] with force offset [" << offset << "] to entity [" << link.Entity() << "] for 1 time step." << std::endl; } @@ -295,15 +299,16 @@ void ApplyLinkWrench::PreUpdate(const UpdateInfo &_info, for (auto msg : this->dataPtr->persistentWrenches) { math::Vector3d force; + math::Vector3d offset; math::Vector3d torque; - auto link = decomposeMessage(_ecm, msg, force, torque); + auto link = decomposeMessage(_ecm, msg, force, torque, offset); if (!link.Valid(_ecm)) { // Not an error, persistent wrenches can be applied preemptively before // an entity is inserted continue; } - link.AddWorldWrench(_ecm, force, torque); + link.AddWorldWrench(_ecm, force, torque, offset); } } diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.hh b/src/systems/apply_link_wrench/ApplyLinkWrench.hh index 625db47578..f2c449e51f 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.hh +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.hh @@ -38,6 +38,9 @@ namespace systems /// that will receive a wrench. If a model is provided, its canonical link /// will receive it. No other entity types are supported. /// + /// Forces and torques are expressed in world coordinates, and the force + /// offset is expressed in the link frame. + /// /// ## Topics /// /// * /world//wrench diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index d78d777c44..7cb97985f5 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -96,15 +96,115 @@ void DetachableJoint::Configure(const Entity &_entity, } // Setup detach topic - std::vector topics; - if (_sdf->HasElement("topic")) + std::vector detachTopics; + if (_sdf->HasElement("detach_topic")) { - topics.push_back(_sdf->Get("topic")); + detachTopics.push_back(_sdf->Get("detach_topic")); } - topics.push_back("/model/" + this->model.Name(_ecm) + + detachTopics.push_back("/model/" + this->model.Name(_ecm) + "/detachable_joint/detach"); - this->topic = validTopic(topics); + if (_sdf->HasElement("topic")) + { + if (_sdf->HasElement("detach_topic")) + { + if (_sdf->Get("topic") != + _sdf->Get("detach_topic")) + { + gzerr << " and tags have different contents. " + "Please verify the correct string and use ." + << std::endl; + } + else + { + gzdbg << "Ignoring tag and using tag." + << std::endl; + } + } + else + { + detachTopics.insert(detachTopics.begin(), + _sdf->Get("topic")); + } + } + + this->detachTopic = validTopic(detachTopics); + if (this->detachTopic.empty()) + { + gzerr << "No valid detach topics for DetachableJoint could be found.\n"; + return; + } + gzdbg << "Detach topic is: " << this->detachTopic << std::endl; + + // Setup subscriber for detach topic + this->node.Subscribe( + this->detachTopic, &DetachableJoint::OnDetachRequest, this); + + gzdbg << "DetachableJoint subscribing to messages on " + << "[" << this->detachTopic << "]" << std::endl; + + // Setup attach topic + std::vector attachTopics; + if (_sdf->HasElement("attach_topic")) + { + attachTopics.push_back(_sdf->Get("attach_topic")); + } + attachTopics.push_back("/model/" + this->model.Name(_ecm) + + "/detachable_joint/attach"); + this->attachTopic = validTopic(attachTopics); + if (this->attachTopic.empty()) + { + gzerr << "No valid attach topics for DetachableJoint could be found.\n"; + return; + } + gzdbg << "Attach topic is: " << this->attachTopic << std::endl; + + // Setup subscriber for attach topic + auto msgCb = std::function( + [this](const auto &) + { + if (this->isAttached){ + gzdbg << "Already attached" << std::endl; + return; + } + this->attachRequested = true; + }); + + if (!this->node.Subscribe(this->attachTopic, msgCb)) + { + gzerr << "Subscriber could not be created for [attach] topic.\n"; + return; + } + + // Setup output topic + std::vector outputTopics; + if (_sdf->HasElement("output_topic")) + { + outputTopics.push_back(_sdf->Get("output_topic")); + } + + outputTopics.push_back("/model/" + this->childModelName + + "/detachable_joint/state"); + + this->outputTopic = validTopic(outputTopics); + if (this->outputTopic.empty()) + { + gzerr << "No valid output topics for DetachableJoint could be found.\n"; + return; + } + gzdbg << "Output topic is: " << this->outputTopic << std::endl; + + // Setup publisher for output topic + this->outputPub = this->node.Advertise( + this->outputTopic); + if (!this->outputPub) + { + gzerr << "Error advertising topic [" << this->outputTopic << "]" + << std::endl; + return; + } + + // Supress Child Warning this->suppressChildWarning = _sdf->Get("suppress_child_warning", this->suppressChildWarning) .first; @@ -118,8 +218,13 @@ void DetachableJoint::PreUpdate( EntityComponentManager &_ecm) { GZ_PROFILE("DetachableJoint::PreUpdate"); - if (this->validConfig && !this->initialized) + // only allow attaching if child entity is detached + if (this->validConfig && !this->isAttached) { + // return if attach is not requested. + if (!this->attachRequested){ + return; + } // Look for the child model and link Entity modelEntity{kNullEntity}; @@ -148,14 +253,11 @@ void DetachableJoint::PreUpdate( this->detachableJointEntity, components::DetachableJoint({this->parentLinkEntity, this->childLinkEntity, "fixed"})); - - this->node.Subscribe( - this->topic, &DetachableJoint::OnDetachRequest, this); - - gzmsg << "DetachableJoint subscribing to messages on " - << "[" << this->topic << "]" << std::endl; - - this->initialized = true; + this->attachRequested = false; + this->isAttached = true; + this->PublishJointState(this->isAttached); + gzdbg << "Attaching entity: " << this->detachableJointEntity + << std::endl; } else { @@ -170,7 +272,8 @@ void DetachableJoint::PreUpdate( } } - if (this->initialized) + // only allow detaching if child entity is attached + if (this->isAttached) { if (this->detachRequested && (kNullEntity != this->detachableJointEntity)) { @@ -179,13 +282,34 @@ void DetachableJoint::PreUpdate( _ecm.RequestRemoveEntity(this->detachableJointEntity); this->detachableJointEntity = kNullEntity; this->detachRequested = false; + this->isAttached = false; + this->PublishJointState(this->isAttached); } } } +////////////////////////////////////////////////// +void DetachableJoint::PublishJointState(bool attached) +{ + msgs::StringMsg detachedStateMsg; + if (attached) + { + detachedStateMsg.set_data("attached"); + } + else + { + detachedStateMsg.set_data("detached"); + } + this->outputPub.Publish(detachedStateMsg); +} + ////////////////////////////////////////////////// void DetachableJoint::OnDetachRequest(const msgs::Empty &) { + if (!this->isAttached){ + gzdbg << "Already detached" << std::endl; + return; + } this->detachRequested = true; } diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 310c4bac1b..e6cb491285 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -36,7 +36,9 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { /// \brief A system that initially attaches two models via a fixed joint and - /// allows for the models to get detached during simulation via a topic. + /// allows for the models to get detached during simulation via a topic. A + /// model can be re-attached during simulation via a topic. The status of the + /// detached state can be monitored via a topic as well. /// /// Parameters: /// @@ -48,7 +50,21 @@ namespace systems /// - ``: Name of the link in the child model to be used in /// creating a fixed joint with a link in the parent model. /// - /// - `` (optional): Topic name to be used for detaching connections + /// - `` (optional): Topic name to be used for detaching connections. + /// Using is preferred. + /// + /// - `` (optional): Topic name to be used for detaching + /// connections. If multiple detachable plugin is used in one model, + /// `detach_topic` is REQUIRED to detach child models individually. + /// + /// - `` (optional): Topic name to be used for attaching + /// connections. If multiple detachable plugin is used in one model, + /// `attach_topic` is REQUIRED to attach child models individually. + /// + /// - `` (optional): Topic name to be used for publishing + /// the state of the detachment. If multiple detachable plugin is used in + /// one model, `output_topic` is REQUIRED to publish child models state + /// individually. /// /// - `` (optional): If true, the system /// will not print a warning message if a child model does not exist yet. @@ -73,6 +89,15 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) final; + /// \brief Gazebo communication node. + private: transport::Node node; + + /// \brief A publisher to send state of the detachment + private: transport::Node::Publisher outputPub; + + /// \brief Helper function to publish the state of the detachment + private: void PublishJointState(bool attached); + /// \brief Callback for detach request topic private: void OnDetachRequest(const msgs::Empty &_msg); @@ -86,7 +111,13 @@ namespace systems private: std::string childLinkName; /// \brief Topic to be used for detaching connections - private: std::string topic; + private: std::string detachTopic; + + /// \brief Topic to be used for re-attaching connections + private: std::string attachTopic; + + /// \brief Topic to be used for publishing detached state + private: std::string outputTopic; /// \brief Whether to suppress warning about missing child model. private: bool suppressChildWarning{false}; @@ -103,14 +134,15 @@ namespace systems /// \brief Whether detachment has been requested private: std::atomic detachRequested{false}; - /// \brief Gazebo communication node. - public: transport::Node node; + /// \brief Whether attachment has been requested + private: std::atomic attachRequested{true}; + + /// \brief Whether child entity is attached + private: std::atomic isAttached{false}; /// \brief Whether all parameters are valid and the system can proceed private: bool validConfig{false}; - /// \brief Whether the system has been initialized - private: bool initialized{false}; }; } } diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 0d0d42d161..db41071a6f 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -70,6 +70,9 @@ class gz::sim::systems::JointPositionControllerPrivate /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; + /// \brief Is the maximum PID gain set. + public: bool isMaxSet {false}; + /// \brief Model interface public: Model model{kNullEntity}; @@ -175,6 +178,7 @@ void JointPositionController::Configure(const Entity &_entity, if (_sdf->HasElement("cmd_max")) { cmdMax = _sdf->Get("cmd_max"); + this->dataPtr->isMaxSet = true; } if (_sdf->HasElement("cmd_min")) { @@ -430,14 +434,14 @@ void JointPositionController::PreUpdate( auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; // Limit the maximum change to maxMovement - if (abs(error) > maxMovement) + if (abs(error) > maxMovement && this->dataPtr->isMaxSet) { targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() : -this->dataPtr->posPid.CmdMax(); } else { - targetVel = -error; + targetVel = - error / dt; } for (Entity joint : this->dataPtr->jointEntities) { diff --git a/src/systems/joint_state_publisher/JointStatePublisher.hh b/src/systems/joint_state_publisher/JointStatePublisher.hh index c83d9a1197..28d2408091 100644 --- a/src/systems/joint_state_publisher/JointStatePublisher.hh +++ b/src/systems/joint_state_publisher/JointStatePublisher.hh @@ -33,8 +33,8 @@ namespace sim inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { - /// \brief The JointStatePub system publishes state information for - /// a model. The published message type is gz::msgs::Model, and the + /// \brief The JointStatePublisher system publishes joint state information + /// for a model. The published message type is gz::msgs::Model, and the /// publication topic is determined by the `` parameter. /// /// By default the JointStatePublisher will publish all joints for @@ -45,7 +45,7 @@ namespace systems /// /// ``: Name of the topic to publish to. This parameter is optional, /// and if not provided, the joint state will be published to - /// "/world//model//state". + /// "/world//model//joint_state". /// ``: Name of a joint to publish. This parameter can be /// specified multiple times, and is optional. All joints in a model will /// be published if joint names are not specified. diff --git a/src/systems/physics/EntityFeatureMap_TEST.cc b/src/systems/physics/EntityFeatureMap_TEST.cc index 948d83851a..40fd0d9abf 100644 --- a/src/systems/physics/EntityFeatureMap_TEST.cc +++ b/src/systems/physics/EntityFeatureMap_TEST.cc @@ -25,6 +25,7 @@ #include #include #include +#include #include #include #include @@ -57,7 +58,7 @@ class EntityFeatureMapFixture: public InternalFixture<::testing::Test> const std::string pluginLib = "gz-physics-dartsim-plugin"; common::SystemPaths systemPaths; - systemPaths.AddPluginPaths({GZ_PHYSICS_ENGINE_INSTALL_DIR}); + systemPaths.AddPluginPaths(gz::physics::getEngineInstallDir()); auto pathToLib = systemPaths.FindSharedLibrary(pluginLib); ASSERT_FALSE(pathToLib.empty()) diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index c70e2103db..33e3db4224 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -31,6 +31,7 @@ #include #include #include +#include #include #include @@ -47,6 +48,7 @@ #include #include #include +#include #include #include @@ -810,7 +812,7 @@ void Physics::Configure(const Entity &_entity, // * Engines installed with gz-physics common::SystemPaths systemPaths; systemPaths.SetPluginPathEnv(this->dataPtr->pluginPathEnv); - systemPaths.AddPluginPaths({GZ_PHYSICS_ENGINE_INSTALL_DIR}); + systemPaths.AddPluginPaths(gz::physics::getEngineInstallDir()); auto pathToLib = systemPaths.FindSharedLibrary(pluginLib); if (pathToLib.empty()) @@ -1406,15 +1408,9 @@ void PhysicsPrivate::CreateCollisionEntities(const EntityComponentManager &_ecm, return true; } - auto &meshManager = *common::MeshManager::Instance(); - auto fullPath = asFullPath(meshSdf->Uri(), meshSdf->FilePath()); - auto *mesh = meshManager.Load(fullPath); - if (nullptr == mesh) - { - gzwarn << "Failed to load mesh from [" << fullPath - << "]." << std::endl; + const common::Mesh *mesh = loadMesh(*meshSdf); + if (!mesh) return true; - } auto linkMeshFeature = this->entityLinkMap.EntityCast(_parent->Data()); @@ -3748,12 +3744,21 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) return; } + // Using ExtraContactData to expose contact Norm, Force & Depth + using Policy = physics::FeaturePolicy3d; + using GCFeature = physics::GetContactsFromLastStepFeature; + using ExtraContactData = GCFeature::ExtraContactDataT; + + // A contact is described by a contactPoint and the corresponding + // extraContactData which we bundle in a pair data structure + using ContactData = std::pair; // Each contact object we get from gz-physics contains the EntityPtrs of the // two colliding entities and other data about the contact such as the - // position. This map groups contacts so that it is easy to query all the + // position and extra contact date (wrench, normal and penetration depth). + // This map groups contacts so that it is easy to query all the // contacts of one entity. - using EntityContactMap = std::unordered_map>; + using EntityContactMap = std::unordered_map>; // This data structure is essentially a mapping between a pair of entities and // a list of pointers to their contact object. We use a map inside a map to @@ -3767,16 +3772,19 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contactComposite : allContacts) { - const auto &contact = contactComposite.Get(); - auto coll1Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision1->EntityID()); - auto coll2Entity = - this->entityCollisionMap.GetByPhysicsId(contact.collision2->EntityID()); + const auto &contactPoint = + contactComposite.Get(); + const auto &extraContactData = contactComposite.Query(); + auto coll1Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision1->EntityID()); + auto coll2Entity = this->entityCollisionMap.GetByPhysicsId( + contactPoint.collision2->EntityID()); if (coll1Entity != kNullEntity && coll2Entity != kNullEntity) { - entityContactMap[coll1Entity][coll2Entity].push_back(&contact); - entityContactMap[coll2Entity][coll1Entity].push_back(&contact); + ContactData data = std::make_pair(&contactPoint, extraContactData); + entityContactMap[coll1Entity][coll2Entity].push_back(data); + entityContactMap[coll2Entity][coll1Entity].push_back(data); } } @@ -3817,9 +3825,36 @@ void PhysicsPrivate::UpdateCollisions(EntityComponentManager &_ecm) for (const auto &contact : contactData) { auto *position = contactMsg->add_position(); - position->set_x(contact->point.x()); - position->set_y(contact->point.y()); - position->set_z(contact->point.z()); + position->set_x(contact.first->point.x()); + position->set_y(contact.first->point.y()); + position->set_z(contact.first->point.z()); + + // Check if the extra contact data exists, + // since not all physics engines support it. + // Then, fill the msg with extra data. + if(contact.second != nullptr) + { + auto *normal = contactMsg->add_normal(); + normal->set_x(contact.second->normal.x()); + normal->set_y(contact.second->normal.y()); + normal->set_z(contact.second->normal.z()); + + auto *wrench = contactMsg->add_wrench(); + auto *body1Wrench = wrench->mutable_body_1_wrench(); + auto *body1Force = body1Wrench->mutable_force(); + body1Force->set_x(contact.second->force.x()); + body1Force->set_y(contact.second->force.y()); + body1Force->set_z(contact.second->force.z()); + + // The force on the second body is equal and opposite + auto *body2Wrench = wrench->mutable_body_2_wrench(); + auto *body2Force = body2Wrench->mutable_force(); + body2Force->set_x(-contact.second->force.x()); + body2Force->set_y(-contact.second->force.y()); + body2Force->set_z(-contact.second->force.z()); + + contactMsg->add_depth(contact.second->depth); + } } } diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index a2f5fd5ead..b9a4efce52 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -268,8 +269,15 @@ class gz::sim::systems::SceneBroadcasterPrivate /// \brief Flag used to indicate if periodic changes need to be published /// This is currently only used in playback mode. public: bool pubPeriodicChanges{false}; + + /// \brief Stores a cache of components that are changed. (This prevents + /// dropping of periodic change components which may not be updated + /// frequently enough) + public: std::unordered_map> changedComponents; }; + ////////////////////////////////////////////////// SceneBroadcaster::SceneBroadcaster() : System(), dataPtr(std::make_unique()) @@ -352,6 +360,9 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // removed entities are removed from the scene graph for the next update cycle this->dataPtr->SceneGraphRemoveEntities(_manager); + // Iterate through entities and their changes to cache them. + _manager.UpdatePeriodicChangeCache(this->dataPtr->changedComponents); + // Publish state only if there are subscribers and // * throttle rate to 60 Hz // * also publish off-rate if there are change events: @@ -394,15 +405,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, else if (!_info.paused) { GZ_PROFILE("SceneBroadcast::PostUpdate UpdateState"); - - if (_manager.HasPeriodicComponentChanges()) - { - auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); - _manager.State(*this->dataPtr->stepMsg.mutable_state(), - {}, periodicComponents); - this->dataPtr->pubPeriodicChanges = false; - } - else + if (!_manager.HasPeriodicComponentChanges()) { // log files may be recorded at lower rate than sim time step. So in // playback mode, the scene broadcaster may not see any periodic @@ -427,6 +430,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // we may be able to remove this in the future and update tests this->dataPtr->stepMsg.mutable_state(); } + + // Apply changes that were caught by the periodic state tracker and then + // clear the change tracker. + _manager.PeriodicStateFromCache(*this->dataPtr->stepMsg.mutable_state(), + this->dataPtr->changedComponents); + this->dataPtr->changedComponents.clear(); } // Full state on demand diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a346076f6d..e9e43b862f 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -17,6 +17,8 @@ #include "Sensors.hh" +#include +#include #include #include #include diff --git a/src/systems/thruster/Thruster.cc b/src/systems/thruster/Thruster.cc index 30b1a3e2b1..8108d0ce7c 100644 --- a/src/systems/thruster/Thruster.cc +++ b/src/systems/thruster/Thruster.cc @@ -14,6 +14,8 @@ * limitations under the License. * */ +#include +#include #include #include #include @@ -144,6 +146,18 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Linear velocity of the vehicle. public: double linearVelocity = 0.0; + /// \brief deadband in newtons + public: double deadband = 0.0; + + /// \brief Flag to enable/disable deadband + public: bool enableDeadband = false; + + /// \brief Mutex to protect enableDeadband + public: std::mutex deadbandMutex; + + /// \brief Topic name used to enable/disable the deadband + public: std::string deadbandTopic = ""; + /// \brief Topic name used to control thrust. Optional public: std::string topic = ""; @@ -159,6 +173,11 @@ class gz::sim::systems::ThrusterPrivateData /// \brief Callback for handling thrust update public: void OnCmdThrust(const msgs::Double &_msg); + /// \brief Callback for handling deadband enable/disable update + /// \param[in] _msg boolean msg to indicate whether to enable or disable + /// the deadband + public: void OnDeadbandEnable(const msgs::Boolean &_msg); + /// \brief Recalculates and updates the thrust coefficient. public: void UpdateThrustCoefficient(); @@ -179,6 +198,12 @@ class gz::sim::systems::ThrusterPrivateData /// \return True if battery is charged, false otherwise. If no battery found, /// returns true. public: bool HasSufficientBattery(const EntityComponentManager &_ecm) const; + + /// \brief Applies the deadband to the thrust and angular velocity by setting + /// those values to zero if the thrust absolute value is below the deadband + /// \param[in] _thrust thrust in N used for check + /// \param[in] _angvel angular velocity in rad/s + public: void ApplyDeadband(double &_thrust, double &_angVel); }; ///////////////////////////////////////////////// @@ -277,6 +302,13 @@ void Thruster::Configure( } } + // Get deadband, default to 0 + if (_sdf->HasElement("deadband")) + { + this->dataPtr->deadband = _sdf->Get("deadband"); + this->dataPtr->enableDeadband = true; + } + // Get a custom topic. if (_sdf->HasElement("topic")) { @@ -312,6 +344,8 @@ void Thruster::Configure( // Subscribe to specified topic for force commands thrusterTopic = gz::transport::TopicUtils::AsValidTopic( ns + "/" + this->dataPtr->topic); + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + ns + "/" + this->dataPtr->topic + "/enable_deadband"); if (this->dataPtr->opmode == ThrusterPrivateData::OperationMode::ForceCmd) { this->dataPtr->node.Subscribe( @@ -347,6 +381,9 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/ang_vel"); + + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } else { @@ -362,11 +399,24 @@ void Thruster::Configure( feedbackTopic = gz::transport::TopicUtils::AsValidTopic( "/model/" + ns + "/joint/" + jointName + "/force"); + + this->dataPtr->deadbandTopic = gz::transport::TopicUtils::AsValidTopic( + "/model/" + ns + "/joint/" + jointName + "/enable_deadband"); } gzmsg << "Thruster listening to commands on [" << thrusterTopic << "]" << std::endl; + if (!this->dataPtr->deadbandTopic.empty()) + { + this->dataPtr->node.Subscribe( + this->dataPtr->deadbandTopic, + &ThrusterPrivateData::OnDeadbandEnable, + this->dataPtr.get()); + gzmsg << "Thruster listening to enable_deadband on [" + << this->dataPtr->deadbandTopic << "]" << std::endl; + } + this->dataPtr->pub = this->dataPtr->node.Advertise( feedbackTopic); @@ -475,6 +525,26 @@ void ThrusterPrivateData::OnCmdThrust(const gz::msgs::Double &_msg) this->propellerAngVel = this->ThrustToAngularVec(this->thrust); } +///////////////////////////////////////////////// +void ThrusterPrivateData::OnDeadbandEnable(const gz::msgs::Boolean &_msg) +{ + std::lock_guard lock(this->deadbandMutex); + if (_msg.data() != this->enableDeadband) + { + if (_msg.data()) + { + gzmsg << "Enabling deadband." << std::endl; + } + else + { + gzmsg << "Disabling deadband." << std::endl; + } + + this->enableDeadband = _msg.data(); + } + +} + ///////////////////////////////////////////////// void ThrusterPrivateData::OnCmdAngVel(const gz::msgs::Double &_msg) { @@ -550,6 +620,16 @@ bool ThrusterPrivateData::HasSufficientBattery( return result; } +///////////////////////////////////////////////// +void ThrusterPrivateData::ApplyDeadband(double &_thrust, double &_angVel) +{ + if (abs(_thrust) < this->deadband) + { + _thrust = 0.0; + _angVel = 0.0; + } +} + ///////////////////////////////////////////////// void Thruster::PreUpdate( const gz::sim::UpdateInfo &_info, @@ -562,6 +642,9 @@ void Thruster::PreUpdate( { return; } + if (!_ecm.HasEntity(this->dataPtr->linkEntity)){ + return; + } // Init battery consumption if it was set if (!this->dataPtr->batteryName.empty() && @@ -631,6 +714,15 @@ void Thruster::PreUpdate( desiredPropellerAngVel = this->dataPtr->propellerAngVel; } + { + std::lock_guard lock(this->dataPtr->deadbandMutex); + if (this->dataPtr->enableDeadband) + { + this->dataPtr->ApplyDeadband(desiredThrust, desiredPropellerAngVel); + } + } + + msgs::Double angvel; // PID control double torque = 0.0; diff --git a/src/systems/thruster/Thruster.hh b/src/systems/thruster/Thruster.hh index 81cbb12e94..5c9cdab562 100644 --- a/src/systems/thruster/Thruster.hh +++ b/src/systems/thruster/Thruster.hh @@ -85,6 +85,11 @@ namespace systems /// [Optional, defaults to 1000N or 1000rad/s] /// - - Minimum input thrust or angular velocity command. /// [Optional, defaults to -1000N or -1000rad/s] + /// - - Deadband of the thruster. Absolute value below which the + /// thruster won't spin nor generate thrust. This value can + /// be changed at runtime using a topic. The topic is either + /// `/model/{ns}/joint/{jointName}/enable_deadband` or + /// `{ns}/{topic}/enable_deadband` depending on other params /// - - Relative speed reduction between the water /// at the propeller (Va) vs behind the vessel. /// [Optional, defults to 0.2] diff --git a/src/systems/touch_plugin/TouchPlugin.cc b/src/systems/touch_plugin/TouchPlugin.cc index 85090e560e..a624e75e83 100644 --- a/src/systems/touch_plugin/TouchPlugin.cc +++ b/src/systems/touch_plugin/TouchPlugin.cc @@ -103,7 +103,7 @@ class gz::sim::systems::TouchPluginPrivate public: bool initialized{false}; /// \brief Set during Load to true if the configuration for the plugin is - /// valid and the post update can run + /// valid and the pre and post update can run public: bool validConfig{false}; /// \brief Whether the plugin is enabled. @@ -373,7 +373,7 @@ void TouchPlugin::Configure(const Entity &_entity, void TouchPlugin::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm) { GZ_PROFILE("TouchPlugin::PreUpdate"); - if (!this->dataPtr->initialized) + if ((!this->dataPtr->initialized) && this->dataPtr->sdfConfig) { // We call Load here instead of Configure because we can't be guaranteed // that all entities have been created when Configure is called @@ -381,9 +381,8 @@ void TouchPlugin::PreUpdate(const UpdateInfo &, EntityComponentManager &_ecm) this->dataPtr->initialized = true; } - // This is not an "else" because "initialized" can be set in the if block - // above - if (this->dataPtr->initialized) + // If Load() was successful, validConfig is set to true + if (this->dataPtr->validConfig) { // Update target entities when new collisions are added std::vector potentialEntities; diff --git a/src/systems/touch_plugin/TouchPlugin.hh b/src/systems/touch_plugin/TouchPlugin.hh index 189bcdbc75..27a4d848aa 100644 --- a/src/systems/touch_plugin/TouchPlugin.hh +++ b/src/systems/touch_plugin/TouchPlugin.hh @@ -32,21 +32,25 @@ namespace systems // Forward declaration class TouchPluginPrivate; - /// \brief Plugin which checks if a model has touched some specific target - /// for a given time continuously and exclusively. After the touch is - /// completed, the plugin is disabled. It can be re-enabled through an - /// Gazebo Transport service. + /// \brief Plugin which publishes a message if the model it is attached + /// to has touched one or more specified targets continuously during a + /// given time. /// - /// It requires that contact sensors be placed in at least one link on the - /// model on which this plugin is attached. + /// After publishing, the plugin is disabled. It can be re-enabled through + /// a Gazebo Transport service call. + /// + /// The plugin requires that a contact sensors is placed in at least one + /// link on the model on which this plugin is attached. /// /// Parameters: /// - /// - `` Scoped name of the desired collision entity that is checked - /// to see if it's touching this model. This can be a substring - /// of the desired collision name so we match more than one - /// collision. For example, using the name of a model will match - /// all its collisions. + /// - `` Name, or substring of a name, that identifies the target + /// collision entity/entities. + /// This value is searched in the scoped name of all collision + /// entities, so it can possibly match more than one collision. + /// For example, using the name of a model will match all of its + /// collisions (scoped name + /// /model_name/link_name/collision_name). /// /// - ` + + 1 0 0 diff --git a/test/worlds/contact_extra_data.sdf b/test/worlds/contact_extra_data.sdf new file mode 100644 index 0000000000..def44661df --- /dev/null +++ b/test/worlds/contact_extra_data.sdf @@ -0,0 +1,105 @@ + + + + + 0 + + + + + + + + 0 0 -9.81 + + + true + + + + + 0 0 1 + 1 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 + + 0 0 0 0 -0 0 + 1 + + 1 + 0 + 0 + 1 + 0 + 1 + + + false + + 0 0 0 0 -0 0 + false + + + + 0 0 0.5 0 0.0 0 + + 1.0 + + 0.4 + 0.4 + 0.4 + + + + + + + 1.0 + + + + + + + 1.0 + + + + + + collision_sphere + /test_extra_collision_data + + 1 + 1000 + + + + + diff --git a/test/worlds/detachable_joint.sdf b/test/worlds/detachable_joint.sdf index d96bbb1ae4..dbbe2604e8 100644 --- a/test/worlds/detachable_joint.sdf +++ b/test/worlds/detachable_joint.sdf @@ -45,7 +45,8 @@ - + body M2 body @@ -76,7 +77,6 @@ - 10 0 1 0 0 0 @@ -120,11 +120,339 @@ - + body1 __model__ body2 + + + 0 2 0.325 0 -0 0 + true + + + -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.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + 0.554282 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + -0.957138 -0.625029 -0.025 -1.5707 0 0 + + 2 + + 0.145833 + 0 + 0 + 0.145833 + 0 + 0.125 + + + + + + 0.3 + + + + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + 0.2 0.2 0.2 1 + + + + + + 0.3 + + + + + + 1 + 1 + 0.035 + 0 + 0 0 1 + + + + + + + + + chassis + front_left_wheel + + 0 0 1 + + -1.79769e+308 + 1.79769e+308 + + + + + + chassis + 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_joint + rear_left_wheel_joint + front_right_wheel_joint + rear_right_wheel_joint + 1.25 + 0.3 + + + chassis + B1 + body + /B1/detach + /B1/attach + + + + + + -1.5 2.35 0.5 0 -0 0 + + + 0.6 + + 0.017 + 0 + 0 + 0.017 + 0 + 0.009 + + + + + + 0.3 0.3 0.5 + + + + 0.0 1.0 0.0 1 + 0.0 1.0 0.0 1 + 0.5 0.5 0.5 1 + + + + + + 0.3 0.3 0.5 + + + + + diff --git a/test/worlds/joint_position_controller_velocity.sdf b/test/worlds/joint_position_controller_velocity.sdf index df66e41742..0b1eaccdd8 100644 --- a/test/worlds/joint_position_controller_velocity.sdf +++ b/test/worlds/joint_position_controller_velocity.sdf @@ -114,6 +114,85 @@ name="gz::sim::systems::JointPositionController"> j1 true + -2.0 + + + + + 100 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 + + + + j2 + true 1000 -2.0 diff --git a/test/worlds/save_world.sdf b/test/worlds/save_world.sdf index dd231b7575..e013dcea65 100644 --- a/test/worlds/save_world.sdf +++ b/test/worlds/save_world.sdf @@ -39,17 +39,17 @@ - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/openroboticstest/models/backpack backpack1 1 0 0 0 0 0 - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack + https://fuel.gazebosim.org/1.0/openroboticstest/models/backpack backpack2 1 2 3 0.1 0.2 0.3 - https://fuel.gazebosim.org/1.0/OpenRobotics/models/Backpack/2 + https://fuel.gazebosim.org/1.0/openroboticstest/models/backpack/3 backpack3 2 0 0 0.1 0.2 0.3 diff --git a/test/worlds/thruster_deadband.sdf b/test/worlds/thruster_deadband.sdf new file mode 100644 index 0000000000..b26facbba4 --- /dev/null +++ b/test/worlds/thruster_deadband.sdf @@ -0,0 +1,113 @@ + + + + + + + 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.0000354167 + 0 + 0 + 0.0000021667 + 0 + 0.0000334167 + + + + + + 0.01 0.25 0.05 + + + + + + + body + propeller + + 1 0 0 + + -1e+12 + 1e+12 + 1e6 + 1e6 + + + + + + deadband + propeller_joint + 0.005 + 950 + 0.25 + 300 + -300 + 50 + 0.05 + + + + + diff --git a/tutorials.md.in b/tutorials.md.in index 39329b050e..10ace242e8 100644 --- a/tutorials.md.in +++ b/tutorials.md.in @@ -21,6 +21,8 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \subpage model_photo_shoot "Model Photo Shoot" Taking perspective, top, front, and side pictures of a model. * \subpage videorecorder "Video Recorder": Record videos from the 3D render window. * \subpage headless_rendering "Headless rendering": Access the GPU on a remote machine to produce sensor data without an X server. +* \subpage apply_force_torque "Apply Force and Torque": Applying forces and/or torques to models during simulation through the GUI. +* \subpage mouse_drag "Mouse Drag": Move models by dragging them in the scene using forces and torques. ### Migration from Gazebo classic @@ -32,6 +34,7 @@ Gazebo @GZ_DESIGNATION_CAP@ library and how to use the library effectively. * \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 migrationsensorapi "Sensor API": Guide on what Sensor C++ functions to call in Gazebo when migrating from Gazebo classic * \subpage ardupilot "Case Study": Migrating the ArduPilot ModelPlugin from Gazebo classic to Gazebo. ## Intermediate diff --git a/tutorials/apply_force_torque.md b/tutorials/apply_force_torque.md new file mode 100644 index 0000000000..b2d5348237 --- /dev/null +++ b/tutorials/apply_force_torque.md @@ -0,0 +1,116 @@ +\page apply_force_torque Apply Force and Torque + +The Apply Force Torque plugin allows users to apply forces and/or torques to +links in the simulation through the graphical user interface. + +## Examples + +Let's go through an example of applying force and torque to simple models. Open +the `shapes.sdf` world with + +```bash +gz sim shapes.sdf +``` + +From the plugin dropdown, select the `Apply Force Torque` plugin. Make sure the +simulation isn't paused. + +![Interface](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/apply_force_torque/Interface.png) + +### Apply force to a link + +We want to apply force to the `cylinder` model. Select the model, either by +clicking on it in the scene or through the Entity Tree. If the model had +multiple links, we could select through the interface which one to apply the +force to (in this case, `cylinder_link`). + +On the dialog, write `10000` on the `X` field under `Force` and click on +`Apply Force`. The cylinder will be pushed on the X direction. The force was +applied in the link's `X` direction for a single time-step, which is in the +order of milliseconds, thus the need for such a large force. + +![Apply Force](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/apply_force_torque/Force.png) + +### Apply torque to a link + +On the dialog, write `2000` on the `X` field under `Torque` and click on +`Apply Torque` to see the cylinder rotate slightly. + +![Apply Torque](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/apply_force_torque/Torque.png) + +### Apply force with an offset + +By default, the force is applied to the link's center of mass, but this can be +modified through the `Offset` fields. On the dialog, write `1000` on the `X` +field under `Force` and `1` under the `Z` field under `Offset`. Press +`Apply Force` to see the model move slightly in the `X` direction while also +rotating around the `Y` direction. + +![Apply Force Offset](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/apply_force_torque/ForceOffset.png) + +### Rotation tool + +On the dialog, write `10000` on the `X` field under `Force`. Click on the force +vector to make the rotation tool appear. Drag the blue circle to rotate the +force around the `Z` axis so that it is aligned with the `Y` direction. Notice +how the XYZ fields changed, but not the magnitude. Press `Apply Force` to see +the model move in the `Y` direction. + +![Rotation tool](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/apply_force_torque/RotationTool.png) + +## The interface explained + +> **Note**: If you apply force and/or torque while the simulation is paused, +they will accumulate and be applied all at once when the simulation is +unpaused. + +### Force + +- **Force X, Y, Z**: Each field specifies how much force will be applied on that +direction, in Newtons (N). The frame is fixed to the link. + +- **Mag**: The total magnitude of the force which will be applied, which is the +Euclidean norm of the 3 forces above. Changing the magnitude changes the XYZ +fields proportionally, maintaining the force direction. + +- **Offset X, Y, Z**: By default, force is applied to the link's center of mass, +in meters. Here you can edit the X, Y and Z fields to give the force an offset +with respect to the center of mass expressed in the link's frame. + + - **Tip**: Right-click the model and choose `View` -> `Center of Mass` to see + its position. You will want to also make the model transparent to see the + center of mass visual (`View` -> `Transparent`). + +- **Apply Force**: Click this to apply only force for one time step. Keep in +mind that time steps are typically in the order of milliseconds, so relatively large +forces are needed in order to apply a significant impulse. + +### Torque + +- **X, Y, Z**: Each field specifies how much torque will be applied about that +axis, in Newton-meters (N.m). The frame is fixed to the link. + +- **Mag**: The total magnitude of the torque which will be applied, which is the +Euclidean norm of the 3 torques above. Changing the magnitude changes the XYZ +fields proportionally, maintaining the torque direction. + +- **Apply Torque**: Click this to apply only torque for one time step. Keep in +mind that time steps are typically in the order of milliseconds, so relatively large +torques are needed in order to apply a significant angular impulse. + + - **Note**: Torque is always applied about the center of mass. + +### Apply All + +Force and torque are applied at the same time, i.e. apply a wrench. + +### Rotation Tool + +The vector (force or torque) directions will always match the directions +specified in the dialog. From the dialog, the direction can be changed by +editing the numbers on the XYZ fields. + +From the scene, select a vector to enable the rotation tool, then drag the +handles. This changes the direction of the vector, updating the XYZ fields +accordingly without modifying its magnitude. You may click again on the vector +to unselect the rotation tool. diff --git a/tutorials/detachable_joints.md b/tutorials/detachable_joints.md index 38f2d0d5a3..2f86d14cd6 100644 --- a/tutorials/detachable_joints.md +++ b/tutorials/detachable_joints.md @@ -7,6 +7,11 @@ models. Because the system uses joints to connect models, the resulting kinematic topology has to be a tree, i.e., kinematic loops are not currently supported. This affects the choice of the parent link, and therefore, the parent model, which is the model that contains the `DetachableJoint` system. +Once detached, the joint can be re-attached by publishing to a topic. +When reattaching, the child model will be attached to the parent model at its +current pose/configuration. To achieve reattachment at a specific pose, the +child model can be positioned accordingly through a set_pose service call prior +to reattaching the joint. For example, [detachable_joint.sdf](https://github.com/gazebosim/gz-sim/blob/ign-gazebo2/examples/worlds/detachable_joint.sdf) demonstrates a four wheel vehicle that holds three objects that are later @@ -36,6 +41,10 @@ need to collide with a parent model or other detached models that have the same parent, the parent model needs to have `` set to true. However, due to an issue in DART, the default physics engine, it is important that none of the parent or child models be in collision in their initial (attached) state. +Furthermore, it is important to note that reattaching a child model is not +currently supported while the child model and parent model are in contact. +Therefore, it is imperative to ensure that there is no collision between the +child and parent model when attempting to perform the reattachment process. The system has the following parameters: @@ -48,6 +57,22 @@ joint. * ``: Name of the link in the `` that will be used as the child link in the detachable joint. -* topic (optional): Topic name to be used for detaching connections. If empty, -a default topic will be created with a pattern -`/model//detachable_joint/detach`. +* `topic` (optional): Topic name to be used for detaching connections. Using + is preferred. If empty, a default topic will be created with a +pattern `/model//detachable_joint/detach`. + +* `detach_topic` (optional): Topic name to be used for detaching connections. + If empty, a default topic will be created with a pattern +`/model//detachable_joint/detach`. If multiple detachable plugin is +used in one model, `detach_topic` is REQUIRED to detach child models individually. + +* `attach_topic` (optional): Topic name to be used for re-attaching connections. + If empty, a default topic will be created with a pattern +`/model//detachable_joint/attach`. If multiple detachable plugin is +used in one model, `attach_topic` is REQUIRED to attach child models individually. + +* `output_topic` (optional): Topic name to be used for publishing the state of +the detachment. If empty, a default topic will be created with a pattern +`/model//detachable_joint/state`. If multiple detachable plugin is +used in one model, `output_topic` is REQUIRED to publish child models state +individually. diff --git a/tutorials/files/apply_force_torque/Force.png b/tutorials/files/apply_force_torque/Force.png new file mode 100755 index 0000000000..bd0153df99 Binary files /dev/null and b/tutorials/files/apply_force_torque/Force.png differ diff --git a/tutorials/files/apply_force_torque/ForceOffset.png b/tutorials/files/apply_force_torque/ForceOffset.png new file mode 100755 index 0000000000..3b4aca2f6f Binary files /dev/null and b/tutorials/files/apply_force_torque/ForceOffset.png differ diff --git a/tutorials/files/apply_force_torque/Interface.png b/tutorials/files/apply_force_torque/Interface.png new file mode 100755 index 0000000000..a764f27999 Binary files /dev/null and b/tutorials/files/apply_force_torque/Interface.png differ diff --git a/tutorials/files/apply_force_torque/RotationTool.png b/tutorials/files/apply_force_torque/RotationTool.png new file mode 100755 index 0000000000..e055749eb3 Binary files /dev/null and b/tutorials/files/apply_force_torque/RotationTool.png differ diff --git a/tutorials/files/apply_force_torque/Torque.png b/tutorials/files/apply_force_torque/Torque.png new file mode 100755 index 0000000000..72155a2864 Binary files /dev/null and b/tutorials/files/apply_force_torque/Torque.png differ diff --git a/tutorials/files/mouse_drag/Interface.png b/tutorials/files/mouse_drag/Interface.png new file mode 100755 index 0000000000..fa7946c07b Binary files /dev/null and b/tutorials/files/mouse_drag/Interface.png differ diff --git a/tutorials/files/mouse_drag/Rotation.png b/tutorials/files/mouse_drag/Rotation.png new file mode 100755 index 0000000000..d1d0c29748 Binary files /dev/null and b/tutorials/files/mouse_drag/Rotation.png differ diff --git a/tutorials/files/mouse_drag/Translation.png b/tutorials/files/mouse_drag/Translation.png new file mode 100755 index 0000000000..3b00c8b723 Binary files /dev/null and b/tutorials/files/mouse_drag/Translation.png differ diff --git a/tutorials/log.md b/tutorials/log.md index 2bedfee3bc..6f88370656 100644 --- a/tutorials/log.md +++ b/tutorials/log.md @@ -42,7 +42,7 @@ Other options for recording: ### From C++ API All features available through the command line are also available through -[gz::sim::ServerConfig](https://gazebosim.org/api/gazebo/2.0/classignition_1_1gazebo_1_1ServerConfig.html). +[gz::sim::ServerConfig](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1ServerConfig.html). When instantiating a server programmatically, logging options can be passed to the constructor, for example: diff --git a/tutorials/migration_link_api.md b/tutorials/migration_link_api.md index ae7890a984..57662d5b76 100644 --- a/tutorials/migration_link_api.md +++ b/tutorials/migration_link_api.md @@ -34,14 +34,14 @@ can be divided in these categories: You'll find the Gazebo APIs below on the following headers: -* [gz/sim/Link.hh](https://gazebosim.org/api/gazebo/3.3/Link_8hh.html) -* [gz/sim/Util.hh](https://gazebosim.org/api/gazebo/3.3/Util_8hh.html) -* [gz/sim/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/3.3/SdfEntityCreator_8hh.html) -* [gz/sim/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +* [gz/sim/Link.hh](https://gazebosim.org/api/gazebo/7/Link_8hh.html) +* [gz/sim/Util.hh](https://gazebosim.org/api/gazebo/7/Util_8hh.html) +* [gz/sim/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/7/SdfEntityCreator_8hh.html) +* [gz/sim/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/3.3/classignition_1_1gazebo_1_1EntityComponentManager.html) +[EntityComponentManager](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1EntityComponentManager.html) directly. The functions presented here exist for convenience and readability. ### Properties diff --git a/tutorials/migration_sensor_api.md b/tutorials/migration_sensor_api.md new file mode 100644 index 0000000000..0616b2b012 --- /dev/null +++ b/tutorials/migration_sensor_api.md @@ -0,0 +1,133 @@ +\page migrationsensorapi + +# Migration from Gazebo-classic: Sensor 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::sensors::Sensor](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1sensors_1_1Sensor.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/). + +## Sensor API + +Gazebo-classic's `gazebo::sensors::Sensor` provides lots of functionality, which +can be divided in these categories: + +* **Properties**: Setting / getting properties + * Example: [Sensor::Name](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1sensors_1_1Sensor.html#a41087c5f2f732f7a2f336b45b952f199) +* **Read family**: Getting parent + * Example: [Sensor::ParentName](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1sensors_1_1Sensor.html#ac39481d8faba2202d0212ef018595de3) +* **Write family**: Changing parent + * Example: [Sensor::SetParent](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1sensors_1_1Sensor.html#a8d07a3535e558a172e212f73b942d39d) +* **Lifecycle**: Functions to control the sensor's lifecycle + * Example: [Sensor::Init](http://osrf-distributions.s3.amazonaws.com/gazebo/api/11.0.0/classgazebo_1_1sensors_1_1Sensor.html#a3e0b39e1326de703012f81ac2be7feba) + +You'll find the Gazebo APIs below on the following headers: + +* [ignition/gazebo/Sensor.hh](https://gazebosim.org/api/gazebo/6/Sensor_8hh.html) +* [ignition/gazebo/Util.hh](https://gazebosim.org/api/gazebo/6/Util_8hh.html) +* [ignition/gazebo/SdfEntityCreator.hh](https://gazebosim.org/api/gazebo/6/SdfEntityCreator_8hh.html) +* [ignition/gazebo/EntityComponentManager.hh](https://gazebosim.org/api/gazebo/6/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/6/classignition_1_1gazebo_1_1EntityComponentManager.html) +directly. + +As an example the `Sensor::Pose()` is a convienient function for querying the `Pose` component from the `EntityComponentManager`, i.e. + +``` + math::Pose3d pose = _ecm.Component(sensorEntityId)->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 Sensor API is related to setting and getting +properties. These functions are great candidates to have equivalents on Gazebo +Gazebo, because the Entity-Component-System architecture is perfect for setting +components (properties) into entities such as sensors. + +--- + +Classic | Gazebo +-- | -- +Category | TODO +FillMsg | TODO +Id | `ignition::gazebo::Sensor::Entity` +IsActive | TODO +LastMeasurementTime | TODO +LastUpdateTime | TODO +Name | `ignition::gazebo::Sensor::Name` +NextRequiredTimestamp | TODO +Noise | TODO +Pose | `ignition::gazebo::Sensor::Pose` +ResetLastUpdateTime | TODO +ScopedName | `ignition::gazebo::scopedName` +SetActive | TODO +SetPose | TODO +SetUpdateRate | TODO +Topic | `ignition::gazebo::Sensor::Topic` +Type | `ignition::gazebo::entityType` +UpdateRate | TODO +Visualize | TODO +WorldName | `ignition::gazebo::worldEntity` + +--- + +## Read family + +These APIs deal with reading information related to parent relationship. + +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 +-- | -- +ParentId | `ignition::gazebo::Sensor::Parent` +ParentName | `ignition::gazebo::Sensor::Parent` + +--- + +## Write family + +These functions deal with modifying the entity tree, attaching children to new +parents. + +--- + +Classic | Gazebo +-- | -- +SetParent | TODO +--- + +## Lifecycle + +These functions aren't related to the state of a sensor, but perform some +processing related to the sensor's lifecycle, like initializing, updating or +terminating it. + +--- + +Classic | Gazebo +-- | -- +ConnectUpdated | TODO +Fini | N/A +Init | N/A +Load | `ignition::gazebo::SdfEntityCreator::CreateEntities` +Update | Entities are updated by systems +--- diff --git a/tutorials/mouse_drag.md b/tutorials/mouse_drag.md new file mode 100644 index 0000000000..cdc4b88bab --- /dev/null +++ b/tutorials/mouse_drag.md @@ -0,0 +1,66 @@ +\page mouse_drag Mouse Drag + +The Mouse Drag plugin allows the user to exert forces and/or torques by dragging +objects in the scene with the mouse cursor. It has two modes: rotation and +translation. + +To use it, open any world (such as `shapes.sdf`) and select `Mouse Drag` from +the plugin dropdown to load the plugin. + +![Interface](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/mouse_drag/Interface.png) + +## Rotation mode + +The rotation mode is activated by Ctrl+Left-clicking and holding a link in the +scene. Dragging the mouse causes a pure torque to be applied, contained in a +plane parallel to the camera. The transparent red bounding box displays the +desired orientation of the link corresponding to the current mouse position. + +The magnitude of the torque is calculated by a spring-damper system with a +constant stiffness and critical damping. It is also proportional to the link's +inertia, so that the same stiffness causes similar effects on different links. +The rotational stiffness can be modified through the interface. + +![Rotation mode](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/mouse_drag/Rotation.png) + +## Translation mode + +The translation mode is activated by Ctrl+Right-clicking and holding a link in +the scene. Dragging the mouse will then move the link towards the mouse +position. On the interface, you may select whether the force should be applied +to the link's center of mass or to the point where the mouse click occured. + +If center of mass is selected, only a force is applied, with a magnitude given +by a constant stiffness and critical damping, scaled by the mass of the link. +The force is always contained in a plane parallel to the camera and passing +through the application point, represented by a transparent gray plane. The +visualization also shows an arrow from the application point to the target +position under the mouse cursor. + +If center of mass is not selected, an additional torque is applied to account +for the offset in the force application point. In this case, the rotation of +the object is also slightly damped (according to the rotational stiffness). +The position stiffness can be modified through the interface. + +![Translation mode](https://raw.githubusercontent.com/gazebosim/gz-sim/main/tutorials/files/mouse_drag/Translation.png) + +## SDF configuration + +The rotation and position stiffness may also be configured in the SDF file. +To do this, add the `` and/or `` to the +plugin element under ``. + +```xml + + + + + 100.0 + 100.0 + + ... + + ... + + +``` diff --git a/tutorials/physics.md b/tutorials/physics.md index 8afb85085e..a8e1c12217 100644 --- a/tutorials/physics.md +++ b/tutorials/physics.md @@ -73,7 +73,7 @@ Alternatively, you can choose a plugin from the command line using the ### From C++ API All features available through the command line are also available through -[gz::sim::ServerConfig](https://gazebosim.org/api/gazebo/2.0/classignition_1_1gazebo_1_1ServerConfig.html). +[gz::sim::ServerConfig](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1ServerConfig.html). When instantiating a server programmatically, a physics engine can be passed to the constructor, for example: diff --git a/tutorials/resources.md b/tutorials/resources.md index 09634ed9b7..9ae4b7d07d 100644 --- a/tutorials/resources.md +++ b/tutorials/resources.md @@ -31,16 +31,16 @@ System plugins may be loaded through: * Attached to a **model**: `` * Attached to a **sensor**: `` * Passing the shared library and class to be loaded through - [PluginInfo](https://gazebosim.org/api/gazebo/4.6/classignition_1_1gazebo_1_1ServerConfig_1_1PluginInfo.html) - (within [ServerConfig](https://gazebosim.org/api/gazebo/4.6/classignition_1_1gazebo_1_1ServerConfig.html)) + [PluginInfo](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1ServerConfig_1_1PluginInfo.html) + (within [ServerConfig](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1ServerConfig.html)) when instantiating the - [Server](https://gazebosim.org/api/gazebo/4.6/classignition_1_1gazebo_1_1Server.html#a084ef7616f5af42061a7aeded5651ab0). + [Server](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1Server.html#a084ef7616f5af42061a7aeded5651ab0). Gazebo will look for system plugins on the following paths, in order: 1. All paths on the `GZ_SIM_SYSTEM_PLUGIN_PATH` environment variable 2. `$HOME/.gz/sim/plugins` -3. [Systems that are installed with Gazebo](https://gazebosim.org/api/gazebo/4.6/namespace gz_1_1gazebo_1_1systems.html) +3. [Systems that are installed with Gazebo](https://gazebosim.org/api/gazebo/7/namespace gz_1_1gazebo_1_1systems.html) ### Gazebo GUI plugins @@ -62,7 +62,7 @@ Gazebo will look for GUI plugins on the following paths, in order: 2. [GUI plugins that are installed with Gazebo](https://github.com/gazebosim/gz-sim/tree/main/src/gui/plugins) 3. Other paths added by calling `gz::gui::App()->AddPluginPath` 4. `~/.gz/gui/plugins` -5. [Plugins which are installed with Gazebo GUI](https://gazebosim.org/api/gui/4.2/namespace gz_1_1gui_1_1plugins.html) +5. [Plugins which are installed with Gazebo GUI](https://gazebosim.org/api/gui/4/namespace gz_1_1gui_1_1plugins.html) ### Physics engines @@ -111,7 +111,7 @@ Top-level entities such as models, lights and actors may be loaded through: * Path / URL to SDF file * (TODO) `gz::msgs::Model`, `gz::msgs::Light` * Within a system, using - [SdfEntityCreator](https://gazebosim.org/api/gazebo/4.6/classignition_1_1gazebo_1_1SdfEntityCreator.html) + [SdfEntityCreator](https://gazebosim.org/api/gazebo/7/classignition_1_1gazebo_1_1SdfEntityCreator.html) or directly creating components and entities. Gazebo will look for URIs (path / URL) in the following, in order: @@ -144,6 +144,14 @@ Gazebo will look for URIs (path / URL) in the following, in order: \* The `GZ_FILE_PATH` environment variable also works in some scenarios, but it's not recommended when using Gazebo. +If a ` starts with the `name://` scheme, +e.g. `name://my_mesh_name`, Gazebo will check to see if a mesh with the +specified name exists in the Mesh Manager and load that mesh if it exists. +This can happen when a `common::Mesh` object is created in memory and +registered with the Mesh Manager via the +[common::MeshManager::Instance()->AddMesh](https://gazebosim.org/api/common/5/classgz_1_1common_1_1MeshManager.html#a2eaddabc3a3109bd8757b2a8b2dd2d01) +call. + ### GUI configuration Gazebo Sim's diff --git a/tutorials/terminology.md b/tutorials/terminology.md index b4e8df5e6c..58e29b40e9 100644 --- a/tutorials/terminology.md +++ b/tutorials/terminology.md @@ -10,14 +10,14 @@ to developers touching the source code. * **Entity**: Every "object" in the world, such as models, links, collisions, visuals, lights, joints, etc. - An entity [is just a numeric ID](namespace gz_1_1gazebo.html#ad83694d867b0e3a9446b535b5dfd208d), + An entity [is just a numeric ID](namespacegz_1_1sim.html#ad83694d867b0e3a9446b535b5dfd208d), and may have several components attached to it. Entity IDs are assigned at runtime. * **Component**: Adds a certain functionality or characteristic (e.g., pose, name, material, etc.) to an entity. Gazebo comes with various - [components](namespace gz_1_1gazebo_1_1components.html) + [components](namespacegz_1_1sim_1_1components.html) ready to be used, such as `Pose` and `Inertial`, and downstream developers can also create their own by inheriting from the [BaseComponent](classignition_1_1gazebo_1_1components_1_1BaseComponent.html) diff --git a/tutorials/triggered_publisher.md b/tutorials/triggered_publisher.md index 0e5bfafae0..8670ab7798 100644 --- a/tutorials/triggered_publisher.md +++ b/tutorials/triggered_publisher.md @@ -132,7 +132,7 @@ indicating where the sensor is on the ground. body box1 box_body - /box1/detach + /box1/detach @@ -227,7 +227,7 @@ static model `trigger` by adding the following to `trigger` body box2 box_body - /box2/detach + /box2/detach ```