diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8b177b4b07..15fd1be29c 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,11 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'gz-sim8' + - 'main' # 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. diff --git a/CMakeLists.txt b/CMakeLists.txt index 828e7c8ad1..ff98ca2607 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 @@ -237,7 +244,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 37462da8b0..bf8f9ded05 100644 --- a/Changelog.md +++ b/Changelog.md @@ -731,6 +731,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 @@ -2974,6 +3141,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) 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/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/src/CMakeLists.txt b/src/CMakeLists.txt index 03f038a9b0..49a9b81580 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -173,13 +173,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/SimulationRunner.cc b/src/SimulationRunner.cc index f19915dcab..02d417e754 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 24affa628b..1d02a900ff 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -203,5 +203,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/gui/plugins/apply_force_torque/ApplyForceTorque.cc b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc index 6d42ddff22..24a607d20f 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.cc @@ -15,6 +15,7 @@ * */ +#include #include #include @@ -26,6 +27,7 @@ #include #include #include +#include #include #include #include @@ -129,12 +131,12 @@ namespace sim /// \brief Torque to be applied in link-fixed frame public: math::Vector3d torque{0.0, 0.0, 0.0}; - /// \brief Offset from the link origin to the center of mass in world coords - public: math::Vector3d inertialPos; - /// \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}; @@ -209,7 +211,14 @@ ApplyForceTorque::ApplyForceTorque() } ///////////////////////////////////////////////// -ApplyForceTorque::~ApplyForceTorque() = default; +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*/) @@ -443,9 +452,8 @@ void ApplyForceTorque::Update(const UpdateInfo &/*_info*/, *this->dataPtr->selectedEntity); if (inertial) { - this->dataPtr->inertialPos = - linkWorldPose.Rot().RotateVector(inertial->Data().Pose().Pos()); this->dataPtr->linkWorldPose = linkWorldPose; + this->dataPtr->inertialPose = inertial->Data().Pose(); } } @@ -499,6 +507,27 @@ void ApplyForceTorque::SetForce(QVector3D _force) 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(); } ///////////////////////////////////////////////// @@ -521,6 +550,33 @@ void ApplyForceTorque::SetTorque(QVector3D _torque) 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); } ///////////////////////////////////////////////// @@ -553,15 +609,21 @@ void ApplyForceTorquePrivate::PublishWrench(bool _applyForce, bool _applyTorque) } // Force and torque in world coordinates - math::Vector3d forceToApply = this->linkWorldPose.Rot().RotateVector( - _applyForce ? this->force : math::Vector3d::Zero); - math::Vector3d torqueToApply = this->linkWorldPose.Rot().RotateVector( - _applyTorque ? this->torque : math::Vector3d::Zero) + - this->inertialPos.Cross(forceToApply); + 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); @@ -632,14 +694,16 @@ void ApplyForceTorquePrivate::OnRender() ///////////////////////////////////////////////// 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 = this->linkWorldPose.Pos() + - this->inertialPos + this->linkWorldPose.Rot().RotateVector(this->offset); + math::Vector3d applicationPoint = + inertialWorldPose.Pos() + + this->linkWorldPose.Rot().RotateVector(this->offset); double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) / 2.0; this->wrenchVis.UpdateVectorVisual( @@ -658,7 +722,7 @@ void ApplyForceTorquePrivate::UpdateVisuals() math::Vector3d worldTorque = this->linkWorldPose.Rot().RotateVector(this->torque); math::Vector3d applicationPoint = - this->linkWorldPose.Pos() + this->inertialPos; + inertialWorldPose.Pos(); double scale = applicationPoint.Distance(this->camera->WorldPose().Pos()) / 2.0; this->wrenchVis.UpdateVectorVisual( @@ -678,13 +742,13 @@ void ApplyForceTorquePrivate::UpdateVisuals() && this->force != math::Vector3d::Zero) { pos = - this->linkWorldPose.Pos() + this->inertialPos + + inertialWorldPose.Pos() + this->linkWorldPose.Rot().RotateVector(this->offset); } else if (this->activeVector == RotationToolVector::TORQUE && this->torque != math::Vector3d::Zero) { - pos = this->linkWorldPose.Pos() + this->inertialPos; + pos = inertialWorldPose.Pos(); } else { @@ -865,7 +929,8 @@ void ApplyForceTorquePrivate::HandleMouseEvents() } /// get start and end pos in world frame from 2d point - math::Vector3d pos = this->linkWorldPose.Pos() + this->inertialPos + + 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); diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh index 7588915082..9c9e219834 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.hh @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -73,6 +74,14 @@ namespace sim NOTIFY ForceChanged ) + /// \brief Force magnitude + Q_PROPERTY( + double forceMag + READ ForceMag + WRITE SetForceMag + NOTIFY ForceMagChanged + ) + /// \brief Torque Q_PROPERTY( QVector3D torque @@ -81,6 +90,14 @@ namespace sim NOTIFY TorqueChanged ) + /// \brief Torque magnitude + Q_PROPERTY( + double torqueMag + READ TorqueMag + WRITE SetTorqueMag + NOTIFY TorqueMagChanged + ) + /// \brief Constructor public: ApplyForceTorque(); @@ -98,24 +115,28 @@ namespace sim 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 @@ -129,6 +150,18 @@ namespace sim /// \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; @@ -140,6 +173,24 @@ namespace sim /// \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(); diff --git a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml index d3cc802760..7a24301135 100644 --- a/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml +++ b/src/gui/plugins/apply_force_torque/ApplyForceTorque.qml @@ -23,7 +23,7 @@ GridLayout { columns: 8 columnSpacing: 10 Layout.minimumWidth: 350 - Layout.minimumHeight: 700 + Layout.minimumHeight: 750 anchors.fill: parent anchors.leftMargin: 10 anchors.rightMargin: 10 @@ -37,8 +37,11 @@ GridLayout { // Precision of the GzSpinBoxes in decimal places property int decimalPlaces: 2 - // Step size of the GzSpinBoxes - property double step: 1.0 + // 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 @@ -59,21 +62,21 @@ GridLayout { Text { Layout.columnSpan: 2 id: modelText - color: "dimgrey" + color: "black" text: qsTr("Model:") } Text { Layout.columnSpan: 6 id: modelName - color: "dimgrey" + color: "black" text: ApplyForceTorque.modelName } Text { Layout.columnSpan: 2 id: linkText - color: "dimgrey" + color: "black" text: qsTr("Link:") } @@ -88,24 +91,33 @@ GridLayout { } } - // Force + // Force and offset Text { - Layout.columnSpan: 8 + Layout.columnSpan: 4 id: forceText - color: "dimgrey" - text: qsTr("Force (applied to the center of mass):") + 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: "dimgrey" + color: "black" text: qsTr("X (N)") } GzSpinBox { - Layout.columnSpan: 6 + Layout.columnSpan: 2 Layout.fillWidth: true id: forceX maximumValue: maxValue @@ -116,16 +128,37 @@ GridLayout { 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: "dimgrey" + color: "black" text: qsTr("Y (N)") } GzSpinBox { - Layout.columnSpan: 6 + Layout.columnSpan: 2 Layout.fillWidth: true id: forceY maximumValue: maxValue @@ -136,16 +169,37 @@ GridLayout { 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: "dimgrey" + color: "black" text: qsTr("Z (N)") } GzSpinBox { - Layout.columnSpan: 6 + Layout.columnSpan: 2 Layout.fillWidth: true id: forceZ maximumValue: maxValue @@ -156,6 +210,47 @@ GridLayout { 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 @@ -169,7 +264,7 @@ GridLayout { Text { Layout.columnSpan: 8 id: torqueText - color: "dimgrey" + color: "black" text: qsTr("Torque:") } @@ -177,7 +272,7 @@ GridLayout { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: torqueXText - color: "dimgrey" + color: "black" text: qsTr("X (N.m)") } @@ -197,7 +292,7 @@ GridLayout { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: torqueYText - color: "dimgrey" + color: "black" text: qsTr("Y (N.m)") } @@ -217,7 +312,7 @@ GridLayout { Layout.columnSpan: 2 horizontalAlignment: Text.AlignRight id: torqueZText - color: "dimgrey" + color: "black" text: qsTr("Z (N.m)") } @@ -233,6 +328,26 @@ GridLayout { 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 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/gz.cc b/src/gz.cc index 3c7772e627..bead69e5dd 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -470,3 +470,8 @@ extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, return gz::sim::gui::runGui(argc, argv, _guiConfig, _file, _waitGui, _renderEngine, _renderEngineGuiApiBackend); } + +int main(int argc, char* argv[]) +{ + return sim::gui::runGui(argc, argv, nullptr); +} 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/WrenchVisualizer.cc b/src/rendering/WrenchVisualizer.cc index 0adab74c16..61203f501f 100644 --- a/src/rendering/WrenchVisualizer.cc +++ b/src/rendering/WrenchVisualizer.cc @@ -15,10 +15,14 @@ * */ +#include + #include #include +#include #include #include +#include #include #include "gz/sim/rendering/WrenchVisualizer.hh" diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index eadb2f47fe..6ff5b1fdf3 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -80,6 +80,8 @@ 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. @@ -149,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")) { @@ -164,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); @@ -183,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); } diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 10d2156870..93e504758d 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 d0280f6560..154b1f8ccc 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/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index ae45b0d1e5..3fe1e5941f 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -42,6 +42,7 @@ #include #include #include +#include #include #include @@ -286,8 +287,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()) @@ -370,6 +378,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: @@ -412,15 +423,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 @@ -445,6 +448,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 b0e93b4b37..3bee1711bd 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/track_controller/TrackController.cc b/src/systems/track_controller/TrackController.cc index 6f1c310c8c..144017e64c 100644 --- a/src/systems/track_controller/TrackController.cc +++ b/src/systems/track_controller/TrackController.cc @@ -25,10 +25,12 @@ #include #include +#include #include #include #include +#include #include #include @@ -140,6 +142,8 @@ class gz::sim::systems::TrackControllerPrivate /// \brief World poses of all collision elements of the track's link. public: std::unordered_map collisionsWorldPose; + /// \brief Track position + public: double position {0}; /// \brief The last commanded velocity. public: double velocity {0}; /// \brief Commanded velocity clipped to allowable range. @@ -148,8 +152,9 @@ class gz::sim::systems::TrackControllerPrivate public: double prevVelocity {0}; /// \brief Second previous clipped commanded velocity. public: double prevPrevVelocity {0}; + /// \brief The point around which the track circles (in world coords). Should - /// be set to +Inf if the track is going straight. + /// be set to Inf or NaN if the track is going straight. public: math::Vector3d centerOfRotation {math::Vector3d::Zero * math::INF_D}; /// \brief protects velocity and centerOfRotation public: std::mutex cmdMutex; @@ -169,6 +174,13 @@ class gz::sim::systems::TrackControllerPrivate /// \brief Limiter of the commanded velocity. public: math::SpeedLimiter limiter; + + /// \brief Odometry message publisher. + public: transport::Node::Publisher odometryPub; + /// \brief Update period calculated from . + public: std::chrono::steady_clock::duration odometryPubPeriod{0}; + /// \brief Last sim time the odometry was published. + public: std::chrono::steady_clock::duration lastOdometryPubTime{0}; }; ////////////////////////////////////////////////// @@ -268,6 +280,26 @@ void TrackController::Configure(const Entity &_entity, gzdbg << "Subscribed to " << corTopic << " for receiving track center " << "of rotation commands." << std::endl; + // Publish track odometry + const auto kDefaultOdometryTopic = topicPrefix + "/odometry"; + const auto odometryTopic = validTopic({_sdf->Get( + "odometry_topic", kDefaultOdometryTopic).first, kDefaultOdometryTopic}); + this->dataPtr->odometryPub = + this->dataPtr->node.Advertise(odometryTopic); + + double odometryFreq = _sdf->Get( + "odometry_publish_frequency", 50).first; + std::chrono::duration odomPer{0.0}; + if (odometryFreq > 0) + { + odomPer = std::chrono::duration(1 / odometryFreq); + this->dataPtr->odometryPubPeriod = + std::chrono::duration_cast(odomPer); + } + gzdbg << "Publishing odometry to " << odometryTopic + << " with period " << odomPer.count() << " seconds." << std::endl; + + this->dataPtr->trackOrientation = _sdf->Get( "track_orientation", math::Quaterniond::Identity).first; @@ -423,6 +455,10 @@ void TrackController::PreUpdate( this->dataPtr->prevVelocity, this->dataPtr->prevPrevVelocity, _info.dt); + // Integrate track position + const double dtSec = std::chrono::duration(_info.dt).count(); + this->dataPtr->position += ( this->dataPtr->limitedVelocity * dtSec ); + this->dataPtr->prevPrevVelocity = this->dataPtr->prevVelocity; this->dataPtr->prevVelocity = this->dataPtr->limitedVelocity; @@ -433,6 +469,71 @@ void TrackController::PreUpdate( } } +////////////////////////////////////////////////// +void TrackController::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager & /*_ecm*/) +{ + // Nothing left to do if paused. + if (_info.paused) + return; + + // Throttle publishing + auto diff = _info.simTime - this->dataPtr->lastOdometryPubTime; + if (diff < this->dataPtr->odometryPubPeriod) + { + return; + } + this->dataPtr->lastOdometryPubTime = _info.simTime; + + + // Construct the odometry message and publish it: + // + // Only odometry info is published (i.e. no other kinematic state info such + // as acceleration or jerk), as these are the only known values. + // E.g. at timestep 'k': + // - For an ideal system: (position k) = (position k-1) + (velocity k-1) * dt, + // - And (velocity k) is known from the velocity command (possibly limited by + // the SpeedLimiter). + // However, since this is a velocity-resolved controler, (acceleration k) + // and (jerk k) are unknown, e.g.: + // (acceleration k) = ( (velocity k+1) - (velocity k) ) / dt + // in which (velocity k+1) is unknown in timestep k. + // + // Note that, in case of a lower publish frequency than the simulation + // frequency, a similar issue exists for the velocity, since only the + // instantaneous velocity is known at each time step, and not the average + // velocity. E.g. consider: + // + // Time 0 1 2 3 4 5 + // Velocity 10 10 10 10 0 0 + // Position 0 10 20 30 40 40 + // + // with publish at: + // - time 0: position 0 and velocity 10 + // - time 5: position 40 and velocity 0 + // + // For '(pos k) = (pos k-1) + (vel k-1) * dt' to hold, with k = time 5 + // and k-1 = time 0, the reported velocity at time 0 should be '8': + // (40 - 0) / 5 = 8 (i.e. the average velocity over time 0 to 5), + // instead of the reported (instantaneous) velocity '10'. + // + // Imo. this error is acceptable, as real life sensors (e.g. encoder and + // resolver) also report instantaneous values for position and velocity. + // + msgs::Odometry msg; + + // Set the time stamp in the header + msg.mutable_header()->mutable_stamp()->CopyFrom( + convert(_info.simTime)); + + // Set position and velocity + msg.mutable_pose()->mutable_position()->set_x(this->dataPtr->position); + msg.mutable_twist()->mutable_linear()->set_x(this->dataPtr->limitedVelocity); + + this->dataPtr->odometryPub.Publish(msg); +} + + ////////////////////////////////////////////////// void TrackControllerPrivate::ComputeSurfaceProperties( const Entity& _collision1, @@ -616,7 +717,8 @@ void TrackControllerPrivate::OnCenterOfRotation(const msgs::Vector3d& _msg) GZ_ADD_PLUGIN(TrackController, System, TrackController::ISystemConfigure, - TrackController::ISystemPreUpdate) + TrackController::ISystemPreUpdate, + TrackController::ISystemPostUpdate) GZ_ADD_PLUGIN_ALIAS(TrackController, "gz::sim::systems::TrackController") diff --git a/src/systems/track_controller/TrackController.hh b/src/systems/track_controller/TrackController.hh index 2e366a15e1..ff0e95f323 100644 --- a/src/systems/track_controller/TrackController.hh +++ b/src/systems/track_controller/TrackController.hh @@ -82,6 +82,14 @@ namespace systems /// rotation command will only act for the given number of seconds and the /// track will be stopped if no command arrives before this timeout. /// + /// `` The topic on which the track odometry (i.e. position + /// and instantaneous velocity) is published. This can be used e.g. to + /// simulate a conveyor with encoder feedback. + /// Defaults to `/model/${model_name}/link/${link_name}/odometry`. + /// + /// `` the frequency (in Hz) at which the + /// odometry messages are published. Defaults to 50 Hz. + /// /// ``/`` Min/max velocity of the track (m/s). /// If not specified, the velocity is not limited (however the physics will, /// in the end, have some implicit limit). @@ -96,7 +104,8 @@ namespace systems class TrackController : public System, public ISystemConfigure, - public ISystemPreUpdate + public ISystemPreUpdate, + public ISystemPostUpdate { /// \brief Constructor public: TrackController(); @@ -112,8 +121,12 @@ namespace systems // Documentation inherited public: void PreUpdate( - const gz::sim::UpdateInfo &_info, - gz::sim::EntityComponentManager &_ecm) override; + const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + + // Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; /// \brief Private data pointer private: std::unique_ptr dataPtr; diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2fedf7eaa2..ee8290a5c4 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -105,6 +105,7 @@ set(tests_needing_display dvl_system_bottom_tracking.cc dvl_system_water_mass_tracking.cc gpu_lidar.cc + markers.cc mesh_uri.cc optical_tactile_plugin.cc reset_sensors.cc @@ -115,6 +116,7 @@ set(tests_needing_display shader_param_system.cc thermal_sensor_system.cc thermal_system.cc + triggered_camera.cc wide_angle_camera.cc ) @@ -153,6 +155,7 @@ gz_build_tests(TYPE INTEGRATION ${tests} LIB_DEPS ${EXTRA_TEST_LIB_DEPS} + ${PROJECT_LIBRARY_TARGET_NAME}-rendering ENVIRONMENT GZ_SIM_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} ) diff --git a/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 8a7223676e..c4f440aa29 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -16,6 +16,10 @@ */ #include + +#include +#include + #include #include #include @@ -213,3 +217,145 @@ TEST_F(DetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(LinksInSameModel)) // the expected distance. EXPECT_GT(b2Poses.front().Pos().Z() - b2Poses.back().Pos().Z(), expDist); } + + ///////////////////////////////////////////////// + // Test for re-attaching a detached joint. This uses the vehicle_blue and B1 + // box models. The B1 model is first detached from the vehicle. Although + // detached, the distance (x-direction) between B1 and vehicle is 1.5, which + // is the default offset. Then, linear velocity of 1.0 is published on the + // `/cmd_vel` topic. After 200 iterations, the B1 model will remain in the same + // position whereas the vehicle will move in the x-direction. Now the + // distance between B1 and the vehicle will be the default offset (1.5) + // in addition to the distance traveled by the vehicle. Next, B1 is re-attached + // to the vehicle. After 200 iterations, we can confirm that B1 has moved with + // the vehicle and the distance traveled by B1 is close to that of the vehicle. + // Therefore, it confirms that B1 is re-attached to the vehicle. + TEST_F(DetachableJointTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(ReAttach)) + { + using namespace std::chrono_literals; + + this->StartServer(common::joinPaths("/test", "worlds", + "detachable_joint.sdf")); + + // A lambda that takes a model name and a mutable reference to a vector of + // poses and returns another lambda that can be passed to + // `Relay::OnPostUpdate`. + auto poseRecorder = + [](const std::string &_modelName, std::vector &_poses) + { + return [&, _modelName](const sim::UpdateInfo &, + const sim::EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, const components::Model *, + const components::Name *_name, + const components::Pose *_pose) -> bool + { + if (_name->Data() == _modelName) + { + EXPECT_NE(kNullEntity, _entity); + _poses.push_back(_pose->Data()); + } + return true; + }); + }; + }; + + std::vector b1Poses, vehiclePoses; + test::Relay testSystem1; + testSystem1.OnPostUpdate(poseRecorder("B1", b1Poses)); + test::Relay testSystem2; + testSystem2.OnPostUpdate(poseRecorder("vehicle_blue", vehiclePoses)); + + this->server->AddSystem(testSystem1.systemPtr); + this->server->AddSystem(testSystem2.systemPtr); + + transport::Node node; + // time required for the child and parent links to be attached + gzdbg << "Initially attaching the links" << std::endl; + const std::size_t nItersInitialize{100}; + this->server->Run(true, nItersInitialize, false); + + // detach the B1 model from the vehicle model + auto pub = node.Advertise("/B1/detach"); + pub.Publish(msgs::Empty()); + std::this_thread::sleep_for(250ms); + const std::size_t nItersAfterDetach{100}; + this->server->Run(true, nItersAfterDetach, false); + + ASSERT_EQ(nItersAfterDetach + nItersInitialize, b1Poses.size()); + ASSERT_EQ(nItersAfterDetach + nItersInitialize, vehiclePoses.size()); + + // Deafult distance between B1 and the vehicle is 1.5. + auto defaultDist = 1.5; + // Although detached, distance (x-axis) between B1 and vehicle should be 1.5. + EXPECT_NEAR(vehiclePoses.back().Pos().X(), + abs(b1Poses.back().Pos().X()) - defaultDist, 0.0001); + + // clear the vectors + b1Poses.clear(); + vehiclePoses.clear(); + + // Move the vehicle along the x-axis with linear speed of x = 1.0. Since B1 + // has been detached, it's just the vehicle moving forward. + auto cmdVelPub = node.Advertise("/model/vehicle_blue/cmd_vel"); + msgs::Twist msg; + msgs::Set(msg.mutable_linear(), math::Vector3d(1.0, 0, 0)); + cmdVelPub.Publish(msg); + std::this_thread::sleep_for(250ms); + const std::size_t nItersAfterMoving{200}; + this->server->Run(true, nItersAfterMoving, false); + + ASSERT_EQ(nItersAfterMoving, b1Poses.size()); + ASSERT_EQ(nItersAfterMoving, vehiclePoses.size()); + + // Model B1 X pos is stationary. Therefore the diff will be close to 0. + EXPECT_TRUE(abs(b1Poses.front().Pos().X() - + b1Poses.back().Pos().X()) < 0.001); + + // Model vehicle_blue X pos will be different since it moved. + auto distTraveled = 0.1; + EXPECT_TRUE(abs(vehiclePoses.front().Pos().X() - + vehiclePoses.back().Pos().X()) > distTraveled); + + // Distance between the B1 and vehicle model confirms that it is detached + // and the vehicle traveled away from B1. + auto totalDist = defaultDist + distTraveled; + EXPECT_TRUE(abs(vehiclePoses.back().Pos().X() - + b1Poses.back().Pos().X()) > totalDist); + + // clear the vectors + b1Poses.clear(); + vehiclePoses.clear(); + + // Now re-attach the B1 model back to the vehicle. B1 will move with the + // vehicle. + auto attachPub = node.Advertise("/B1/attach"); + attachPub.Publish(msgs::Empty()); + std::this_thread::sleep_for(250ms); + const std::size_t nItersAfterMovingTogether{200}; + this->server->Run(true, nItersAfterMovingTogether, false); + + ASSERT_EQ(nItersAfterMovingTogether, b1Poses.size()); + ASSERT_EQ(nItersAfterMovingTogether, vehiclePoses.size()); + + // Model B1 should move along with the vehicle. Therefore the position should + // change. + EXPECT_TRUE(abs(b1Poses.front().Pos().X() - + b1Poses.back().Pos().X()) > distTraveled); + + // distance traveled along the x-axis by the B1 model + auto distTraveledB1 = abs(b1Poses.back().Pos().X() - + b1Poses.front().Pos().X()); + + // distance traveled along the x-axis by the vehicle model + auto distTraveledVehicle = abs(vehiclePoses.back().Pos().X() - + vehiclePoses.front().Pos().X()); + gzdbg << "dist by B1: " << distTraveledB1 << " ,dist by vehicle: " + << distTraveledVehicle << ", diff: " + << abs(distTraveledB1 - distTraveledVehicle) << std::endl; + + // since the two models are attached, the distances traveled by both objects + // should be close. + EXPECT_TRUE(abs(distTraveledB1 - distTraveledVehicle) < 0.01); + } diff --git a/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index 0df56e165f..3baca3f04c 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -253,8 +253,8 @@ TEST_F(JointPositionControllerTestFixture, const components::Name *_name, const components::JointPosition *_position) -> bool { - EXPECT_EQ(_name->Data(), jointName); - currentPosition = _position->Data(); + if (_name->Data() == jointName) + currentPosition = _position->Data(); return true; }); }); @@ -267,7 +267,7 @@ TEST_F(JointPositionControllerTestFixture, EXPECT_NEAR(0, currentPosition.at(0), TOL); // joint moves to initial_position at -2.0 - const std::size_t initPosIters = 1000; + const std::size_t initPosIters = 1; server.Run(true, initPosIters, false); double expectedInitialPosition = -2.0; EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL); @@ -283,14 +283,15 @@ TEST_F(JointPositionControllerTestFixture, pub.Publish(msg); // Wait for the message to be published - std::this_thread::sleep_for(100ms); + std::this_thread::sleep_for(1ms); - const std::size_t testIters = 1000; - server.Run(true, testIters , false); + const std::size_t testIters = 1; + server.Run(true, testIters, false); EXPECT_NEAR(targetPosition, currentPosition.at(0), TOL); } + ///////////////////////////////////////////////// // Tests that the JointPositionController accepts joint position // sub_topic commands @@ -481,3 +482,100 @@ TEST_F(JointPositionControllerTestFixture, // joint21 should be at target position EXPECT_NEAR(targetPosition, joint21Position.at(0), TOL); } + +///////////////////////////////////////////////// +// Tests that the JointPositionController respects the maximum command +TEST_F(JointPositionControllerTestFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(JointPositonVelocityCommandWithMax)) +{ + using namespace std::chrono_literals; + + // Start server + ServerConfig serverConfig; + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "joint_position_controller_velocity.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + server.SetUpdatePeriod(0ns); + + const std::string jointName = "j2"; + + test::Relay testSystem; + std::vector currentPosition; + testSystem.OnPreUpdate( + [&](const UpdateInfo &, EntityComponentManager &_ecm) + { + auto joint = _ecm.EntityByComponents(components::Joint(), + components::Name(jointName)); + // Create a JointPosition component if it doesn't exist. This signals + // physics system to populate the component + if (nullptr == _ecm.Component(joint)) + { + _ecm.CreateComponent(joint, components::JointPosition()); + } + }); + + testSystem.OnPostUpdate([&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &, + const components::Joint *, + const components::Name *_name, + const components::JointPosition *_position) -> bool + { + if(_name->Data() == jointName) + currentPosition = _position->Data(); + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // joint pos starts at 0 + const std::size_t initIters = 1; + server.Run(true, initIters, false); + EXPECT_NEAR(0, currentPosition.at(0), TOL); + + // joint moves to initial_position at -2.0 + const std::size_t initPosIters = 2; + server.Run(true, initPosIters, false); + double expectedInitialPosition = -2.0; + EXPECT_NEAR(expectedInitialPosition, currentPosition.at(0), TOL); + + // Publish command and check that the joint position is set + transport::Node node; + auto pub = node.Advertise( + "/model/joint_position_controller_test_with_max/joint/j2/0/cmd_pos"); + + const double targetPosition{2.0}; + msgs::Double msg; + msg.set_data(targetPosition); + + int sleep{0}; + int maxSleep{30}; + for (; !pub.HasConnections() && sleep < maxSleep; ++sleep) { + std::this_thread::sleep_for(100ms); + } + + pub.Publish(msg); + + // Wait for the message to be published + std::this_thread::sleep_for(1ms); + + const std::size_t testInitialIters = 1; + server.Run(true, testInitialIters , false); + + // We should not have reached our target yet. + EXPECT_GT(fabs(currentPosition.at(0) - targetPosition), TOL); + + // Eventually reach target + const std::size_t testIters = 1000; + server.Run(true, testIters , false); + EXPECT_NEAR(currentPosition.at(0), targetPosition, TOL); +} diff --git a/test/integration/markers.cc b/test/integration/markers.cc new file mode 100644 index 0000000000..4348c2afce --- /dev/null +++ b/test/integration/markers.cc @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include +#include + +#include +#include +#include +#include +#include "gz/rendering/Scene.hh" +#include +#include + +#include "gz/sim/rendering/MarkerManager.hh" +#include "gz/sim/Server.hh" +#include "test_config.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace gz::sim; + +/// \brief Test MarkersTest system +class MarkersTest : public InternalFixture<::testing::Test> +{ +}; + +std::mutex mutex; +std::vector markerMsgs; + +///////////////////////////////////////////////// +void markerCb(const msgs::Marker &_msg) +{ + mutex.lock(); + markerMsgs.push_back(_msg); + mutex.unlock(); +} + +///////////////////////////////////////////////// +TEST_F(MarkersTest, MarkerPublisher) +{ + std::map params; + auto engine = gz::rendering::engine("ogre2", params); + auto scene = engine->CreateScene("testscene"); + + gz::msgs::Marker markerMsg; + + // Function that Waits for a message to be received + auto wait = [&](std::size_t _size, gz::msgs::Marker &_markerMsg) { + for (int sleep = 0; sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + mutex.lock(); + bool received = markerMsgs.size() == _size; + mutex.unlock(); + + if (received) + break; + } + + mutex.lock(); + EXPECT_EQ(markerMsgs.size(), _size); + auto lastMsg = markerMsgs.back(); + EXPECT_EQ(_markerMsg.DebugString(), lastMsg.DebugString()); + mutex.unlock(); + }; + + + MarkerManager markerManager; + markerManager.Init(scene); + + // subscribe to marker topic + transport::Node node; + node.Subscribe("/marker", &markerCb); + + // Send a request, wait for a message + markerMsg.set_ns("default"); + markerMsg.set_id(0); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::SPHERE); + markerMsg.set_visibility(gz::msgs::Marker::GUI); + node.Request("/marker", markerMsg); + markerManager.Update(); + wait(1, markerMsg); + + // Update without a new message + markerManager.Update(); + + // Send another request, and check that there are two messages + markerMsg.set_ns("default2"); + markerMsg.set_id(1); + markerMsg.set_action(gz::msgs::Marker::ADD_MODIFY); + markerMsg.set_type(gz::msgs::Marker::BOX); + markerMsg.set_visibility(gz::msgs::Marker::GUI); + node.Request("/marker", markerMsg); + markerManager.Update(); + wait(2, markerMsg); +} diff --git a/test/integration/sensors_system_update_rate.cc b/test/integration/sensors_system_update_rate.cc index 95a20aecbe..9df4de1d86 100644 --- a/test/integration/sensors_system_update_rate.cc +++ b/test/integration/sensors_system_update_rate.cc @@ -80,8 +80,9 @@ TEST_F(SensorsFixture, GZ_UTILS_TEST_DISABLED_ON_MAC(UpdateRate)) { gz::sim::ServerConfig serverConfig; - const std::string sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/sensor.sdf"; + const std::string sdfFile = + common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "sensor.sdf"); serverConfig.SetSdfFile(sdfFile); diff --git a/test/integration/tracked_vehicle_system.cc b/test/integration/tracked_vehicle_system.cc index f94bc7c650..69b7155f07 100644 --- a/test/integration/tracked_vehicle_system.cc +++ b/test/integration/tracked_vehicle_system.cc @@ -68,6 +68,28 @@ void verifyPose(const math::Pose3d& pose1, const math::Pose3d& pose2) EXPECT_ANGLE_NEAR(pose1.Rot().Yaw(), pose2.Rot().Yaw(), 1e-1); } +/// \brief Helper function to wait until a predicate is true or a timeout occurs +/// \tparam Pred Predicate function of type bool() +/// \param[in] _timeoutMs Timeout in milliseconds +template +bool waitUntil(int _timeoutMs, Pred _pred) +{ + using namespace std::chrono; + auto tStart = steady_clock::now(); + auto sleepDur = milliseconds(std::min(100, _timeoutMs)); + auto waitDuration = milliseconds(_timeoutMs); + while (duration_cast(steady_clock::now() - tStart) < + waitDuration) + { + if (_pred()) + { + return true; + } + std::this_thread::sleep_for(sleepDur); + } + return false; +} + /// \brief Test TrackedVehicle system. This test drives a tracked robot over a /// course of obstacles and verifies that it is able to climb on/over them. class TrackedVehicleTest : public InternalFixture<::testing::Test> @@ -448,7 +470,8 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> /// \param[in] _sdfFile SDF file to load. /// \param[in] _cmdVelTopic Command velocity topic. protected: void TestConveyor(const std::string &_sdfFile, - const std::string &_cmdVelTopic) + const std::string &_cmdVelTopic, + const std::string &_odometryTopic) { // Start server ServerConfig serverConfig; @@ -458,6 +481,22 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_FALSE(server.Running()); EXPECT_FALSE(*server.Running(0)); + // gz::transport node + transport::Node node; + + // subscribe to odometry + msgs::Odometry odometryMsg; + unsigned int odomMsgCounter = 0; + + auto msgCb = std::function( + [&odometryMsg, &odomMsgCounter](const auto & msg) + { + odometryMsg = msg; + odomMsgCounter++; + }); + + node.Subscribe(_odometryTopic, msgCb); + // Create a system that records the vehicle poses test::Relay ecmGetterSystem; EntityComponentManager* ecm {nullptr}; @@ -501,8 +540,12 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 50 == odomMsgCounter;}); + // Poses for 1s ASSERT_EQ(1000u, poses.size()); + ASSERT_EQ(50u, odomMsgCounter); // check that the box has not moved in X and Y directions (it will move in // Z as it falls down on the conveyor) @@ -512,10 +555,25 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0, odometryMsg.pose().position().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + poses.clear(); + odomMsgCounter = 0; // Publish command and check that vehicle moved - transport::Node node; auto pub = node.Advertise(_cmdVelTopic); // In this test, there is a long conveyor and a small box at its center. @@ -531,8 +589,12 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 50 == odomMsgCounter;}); + // Poses for 1s ASSERT_EQ(1000u, poses.size()); + ASSERT_EQ(50u, odomMsgCounter); EXPECT_NEAR(0.125, poses.back().Pos().X(), 1e-1); EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); @@ -541,10 +603,29 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0.125, odometryMsg.pose().position().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0.25, odometryMsg.twist().linear().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 100 == odomMsgCounter;}); + // Poses for 2s ASSERT_EQ(2000u, poses.size()); + ASSERT_EQ(100u, odomMsgCounter); EXPECT_NEAR(0.5, poses.back().Pos().X(), 1e-1); EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); @@ -553,10 +634,29 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0.5, odometryMsg.pose().position().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0.5, odometryMsg.twist().linear().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + server.Run(true, 1000, false); + // Wait for messages + waitUntil(5000, [&]{return 150 == odomMsgCounter;}); + // Poses for 3s ASSERT_EQ(3000u, poses.size()); + ASSERT_EQ(150u, odomMsgCounter); EXPECT_NEAR(0.875, poses.back().Pos().X(), 1e-1); EXPECT_NEAR(poses[0].Pos().Y(), poses.back().Pos().Y(), 1e-2); @@ -565,6 +665,21 @@ class TrackedVehicleTest : public InternalFixture<::testing::Test> EXPECT_ANGLE_NEAR(poses.back().Rot().Pitch(), 0, 1e-3); EXPECT_ANGLE_NEAR(poses.back().Rot().Yaw(), 0, 1e-3); + // check reported odometry pose and twist + EXPECT_NEAR(0.875, odometryMsg.pose().position().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.pose().position().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().position().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.pose().orientation().w(), 1e-6); + EXPECT_NEAR(0.25, odometryMsg.twist().linear().x(), 1e-1); + EXPECT_NEAR(0, odometryMsg.twist().linear().y(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().linear().z(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().x(), 1e-6); + EXPECT_NEAR(0, odometryMsg.twist().angular().z(), 1e-6); + poses.clear(); } }; @@ -586,5 +701,6 @@ TEST_F(TrackedVehicleTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(Conveyor)) this->TestConveyor( std::string(PROJECT_SOURCE_PATH) + "/test/worlds/conveyor.sdf", - "/model/conveyor/link/base_link/track_cmd_vel"); + "/model/conveyor/link/base_link/track_cmd_vel", + "/model/conveyor/link/base_link/odometry"); } diff --git a/test/integration/triggered_camera.cc b/test/integration/triggered_camera.cc index 782bd2b4cd..14dc2cde80 100644 --- a/test/integration/triggered_camera.cc +++ b/test/integration/triggered_camera.cc @@ -16,6 +16,7 @@ */ #include +#include #ifdef _MSC_VER #pragma warning(push, 0) @@ -33,7 +34,7 @@ #include #include #include -#include +#include #include "plugins/MockSystem.hh" #include "../helpers/EnvTestFixture.hh" 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/tutorials.md.in b/tutorials.md.in index 8ace81b313..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 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/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/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 ```