diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1959936c78..97824d19bf 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -1,6 +1,10 @@ name: Ubuntu CI -on: [push, pull_request] +on: + pull_request: + push: + branches: + - 'gz-sim7' # Every time you make a push to your PR, it cancel immediately the previous checks, # and start a new one. The other runner will be available more quickly to your PR. diff --git a/CMakeLists.txt b/CMakeLists.txt index 5644db73f7..988eb27539 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,12 +38,19 @@ else() set (EXTRA_TEST_LIB_DEPS) endif() -include(test/find_dri.cmake) -FindDRI() +# We're disabling pybind11 by default on Windows because they +# don't have active CI on them for now. +set(skip_pybind11_default_value OFF) +if (MSVC) + set(skip_pybind11_default_value ON) +endif() option(SKIP_PYBIND11 "Skip generating Python bindings via pybind11" - OFF) + ${skip_pybind11_default_value}) + +include(test/find_dri.cmake) +FindDRI() include(CMakeDependentOption) cmake_dependent_option(USE_SYSTEM_PATHS_FOR_PYTHON_INSTALLATION @@ -236,7 +243,7 @@ add_subdirectory(examples) #============================================================================ gz_create_packages() -if (${pybind11_FOUND}) +if (pybind11_FOUND) add_subdirectory(python) endif() #============================================================================ diff --git a/Changelog.md b/Changelog.md index ff67fb15a8..56f17f3edf 100644 --- a/Changelog.md +++ b/Changelog.md @@ -727,6 +727,173 @@ ## Gazebo Sim 6.x + +### Gazebo Sim 6.15.0 (2023-08-16) + +1. Fix Joint Position Controller Behaviour Described in #1997 + * [Pull request #2001](https://github.com/gazebosim/gz-sim/pull/2001) + +1. Fix a minor issue in the documentation of the server API + * [Pull request #2067](https://github.com/gazebosim/gz-sim/pull/2067) + +1. Use sdf::Element::FindElement instead of GetElement in ApplyLinkWrench + * [Pull request #2052](https://github.com/gazebosim/gz-sim/pull/2052) + +1. Backport sensors system threading optimization changes + * [Pull request #2058](https://github.com/gazebosim/gz-sim/pull/2058) + +1. Adds a warning if the `Server` method of a `TestFixture` is called before `Finalize` + * [Pull request #2047](https://github.com/gazebosim/gz-sim/pull/2047) + +1. Protobuf: Do not require version 3 do support Protobuf 4.23.2 (23.2) + * [Pull request #2006](https://github.com/gazebosim/gz-sim/pull/2006) + +1. Disable pybind11 on Windows by default + * [Pull request #2005](https://github.com/gazebosim/gz-sim/pull/2005) + +1. Print an error message when trying to load SDF files that don't contain a `` + * [Pull request #1998](https://github.com/gazebosim/gz-sim/pull/1998) + +1. Port record topic fix + * [Pull request #2004](https://github.com/gazebosim/gz-sim/pull/2004) + +1. Allow re-attaching detached joint + * [Pull request #1687](https://github.com/gazebosim/gz-sim/pull/1687) + +1. Enable GzWeb visualization of markers by republishing service requests on a topic + * [Pull request #1994](https://github.com/gazebosim/gz-sim/pull/1994) + +1. Small fixes to gz headers + * [Pull request #1985](https://github.com/gazebosim/gz-sim/pull/1985) + +1. Speed up Resource Spawner load time by fetching model list asynchronously + * [Pull request #1962](https://github.com/gazebosim/gz-sim/pull/1962) + +1. Use ignition::gazebo:: in class instantiation + * [Pull request #1967](https://github.com/gazebosim/gz-sim/pull/1967) + +1. Add missing cmake exports from core library + * [Pull request #1978](https://github.com/gazebosim/gz-sim/pull/1978) + +1. Add tutorial on migrating the Sensor class from gazebo classic + * [Pull request #1930](https://github.com/gazebosim/gz-sim/pull/1930) + +1. Add tutorial on migrating the Actor class from gazebo classic + * [Pull request #1929](https://github.com/gazebosim/gz-sim/pull/1929) + +1. Fix use of actors that only has trajectory animation + * [Pull request #1947](https://github.com/gazebosim/gz-sim/pull/1947) + +1. Add tutorial on migrating the Joint class from gazebo classic + * [Pull request #1925](https://github.com/gazebosim/gz-sim/pull/1925) + +1. Add tutorial on migrating the Light class from gazebo classic + * [Pull request #1931](https://github.com/gazebosim/gz-sim/pull/1931) + +1. Infrastructure + * [Pull request #1988](https://github.com/gazebosim/gz-sim/pull/1988) + * [Pull request #1940](https://github.com/gazebosim/gz-sim/pull/1940) + +1. Rename COPYING to LICENSE + * [Pull request #1937](https://github.com/gazebosim/gz-sim/pull/1937) + +1. Add Light class + * [Pull request #1918](https://github.com/gazebosim/gz-sim/pull/1918) + +1. Resolve inconsistent visibility on ign-gazebo6 + * [Pull request #1914](https://github.com/gazebosim/gz-sim/pull/1914) + +1. Relax msg count check in RF comms integration test + * [Pull request #1920](https://github.com/gazebosim/gz-sim/pull/1920) + +1. Add Actor class + * [Pull request #1913](https://github.com/gazebosim/gz-sim/pull/1913) + +1. Add Sensor class + * [Pull request #1912](https://github.com/gazebosim/gz-sim/pull/1912) + +1. Allow to change camera user hfov in camera_view plugin + * [Pull request #1807](https://github.com/gazebosim/gz-sim/pull/1807) + +1. Add Joint class + * [Pull request #1910](https://github.com/gazebosim/gz-sim/pull/1910) + +1. Add SensorTopic component to rendering sensors + * [Pull request #1908](https://github.com/gazebosim/gz-sim/pull/1908) + +1. Use a queue to track component registration from mulitiple sources + * [Pull request #1836](https://github.com/gazebosim/gz-sim/pull/1836) + +1. Document behaviour changes introduced #1784 + * [Pull request #1888](https://github.com/gazebosim/gz-sim/pull/1888) + +1. Partial backport of 1728 + * [Pull request #1901](https://github.com/gazebosim/gz-sim/pull/1901) + +1. Fix triggered camera test by waiting for rendering / scene to be ready + * [Pull request #1895](https://github.com/gazebosim/gz-sim/pull/1895) + +1. Backport portion of #1771 to fix command-line test + * [Pull request #1771](https://github.com/gazebosim/gz-sim/pull/1771) + +1. cmdsim.rb: fix ruby syntax + * [Pull request #1884](https://github.com/gazebosim/gz-sim/pull/1884) + +1. Fix some windows warnings (C4244 and C4305) + * [Pull request #1874](https://github.com/gazebosim/gz-sim/pull/1874) + +1. Minor optimization to transform control tool + * [Pull request #1854](https://github.com/gazebosim/gz-sim/pull/1854) + +1. Inherit material cast shadows property + * [Pull request #1856](https://github.com/gazebosim/gz-sim/pull/1856) + +1. Fix record topic + * [Pull request #1855](https://github.com/gazebosim/gz-sim/pull/1855) + +1. Remove duplicate Fuel server used by ResourceSpawner + * [Pull request #1830](https://github.com/gazebosim/gz-sim/pull/1830) + +1. Re-add namespace + * [Pull request #1826](https://github.com/gazebosim/gz-sim/pull/1826) + +1. Fix QML warnings regarding binding loops + * [Pull request #1829](https://github.com/gazebosim/gz-sim/pull/1829) + +1. Update documentation on `UpdateInfo::realTime` + * [Pull request #1817](https://github.com/gazebosim/gz-sim/pull/1817) + +1. Add jennuine as GUI codeowner + * [Pull request #1800](https://github.com/gazebosim/gz-sim/pull/1800) + +1. remove PlotIcon + * [Pull request #1658](https://github.com/gazebosim/gz-sim/pull/1658) + +1. ign -> gz + * [Pull request #1983](https://github.com/gazebosim/gz-sim/pull/1983) + * [Pull request #1646](https://github.com/gazebosim/gz-sim/pull/1646) + * [Pull request #1760](https://github.com/gazebosim/gz-sim/pull/1760) + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1759) + * [Pull request #1758](https://github.com/gazebosim/gz-sim/pull/1758) + * [Pull request #1757](https://github.com/gazebosim/gz-sim/pull/1757) + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1749) + +1. Added collection name to About Dialog + * [Pull request #1756](https://github.com/gazebosim/gz-sim/pull/1756) + +1. Citadel: Removed warnings + * [Pull request #1753](https://github.com/gazebosim/gz-sim/pull/1753) + +1. Remove actors from screen when they are supposed to + * [Pull request #1699](https://github.com/gazebosim/gz-sim/pull/1699) + +1. Readd namespaces for Q_ARGS + * [Pull request #1670](https://github.com/gazebosim/gz-sim/pull/1670) + +1. Remove redundant namespace references + * [Pull request #1635](https://github.com/gazebosim/gz-sim/pull/1635) + + ### Gazebo Sim 6.14.0 (2022-12-29) 1. Fix Ackermann plugin zero linVel turningRadius bug @@ -2970,6 +3137,70 @@ ## Gazebo Sim 3.x +### Gazebo Sim 3.15.0 (2023-05-08) + +1. Speed up Resource Spawner load time by fetching model list asynchronously + * [Pull request #1962](https://github.com/gazebosim/gz-sim/pull/1962) + +1. ign -> gz Migrate Ignition Headers : gz-sim + * [Pull request #1646](https://github.com/gazebosim/gz-sim/pull/1646) + * [Pull request #1967](https://github.com/gazebosim/gz-sim/pull/1967) + * [Pull request #1978](https://github.com/gazebosim/gz-sim/pull/1978) + * [Pull request #1983](https://github.com/gazebosim/gz-sim/pull/1983) + * [Pull request #1985](https://github.com/gazebosim/gz-sim/pull/1985) + +1. Infrastructure + * [Pull request #1940](https://github.com/gazebosim/gz-sim/pull/1940) + * [Pull request #1937](https://github.com/gazebosim/gz-sim/pull/1937) + +1. Backport portion of #1771 to fix command-line test + * [Pull request #1771](https://github.com/gazebosim/gz-sim/pull/1771) + +1. cmdsim.rb: fix ruby syntax + * [Pull request #1884](https://github.com/gazebosim/gz-sim/pull/1884) + +1. Fix loading wold with record topic + * [Pull request #1855](https://github.com/gazebosim/gz-sim/pull/1855) + +1. Remove duplicate Fuel server used by ResourceSpawner + * [Pull request #1830](https://github.com/gazebosim/gz-sim/pull/1830) + +1. Re-add namespace for GUI render event + * [Pull request #1826](https://github.com/gazebosim/gz-sim/pull/1826) + +1. Fix QML warnings regarding binding loops + * [Pull request #1829](https://github.com/gazebosim/gz-sim/pull/1829) + +1. Update documentation on `UpdateInfo::realTime` + * [Pull request #1817](https://github.com/gazebosim/gz-sim/pull/1817) + +1. Add jennuine as GUI codeowner + * [Pull request #1800](https://github.com/gazebosim/gz-sim/pull/1800) + +1. Remove plotIcon in Physics.qml for Component Inspector + * [Pull request #1658](https://github.com/gazebosim/gz-sim/pull/1658) + +1. Convert ignitionrobotics to gazebosim in tutorials + * [Pull request #1757](https://github.com/gazebosim/gz-sim/pull/1757) + * [Pull request #1758](https://github.com/gazebosim/gz-sim/pull/1758) + * [Pull request #1759](https://github.com/gazebosim/gz-sim/pull/1759) + * [Pull request #1760](https://github.com/gazebosim/gz-sim/pull/1760) + +1. Added collection name to About Dialog + * [Pull request #1756](https://github.com/gazebosim/gz-sim/pull/1756) + +1. Remove compiler warnings + * [Pull request #1753](https://github.com/gazebosim/gz-sim/pull/1753) + +1. Update examples to use gazebosim.org + * [Pull request #1749](https://github.com/gazebosim/gz-sim/pull/1749) + +1. Remove actors from screen when they are supposed to + * [Pull request #1699](https://github.com/gazebosim/gz-sim/pull/1699) + +1. Readd namespaces for Q_ARGS + * [Pull request #1670](https://github.com/gazebosim/gz-sim/pull/1670) + ### Gazebo Sim 3.X.X (20XX-XX-XX) ### Gazebo Sim 3.13.0 (2022-06-01) 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 fe188f1c48..37486f94cc 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -160,13 +160,17 @@ gz_add_component(gz ${cli_sources} GET_TARGET_NAME gz_lib_target) target_link_libraries(${gz_lib_target} - PRIVATE + PUBLIC ${PROJECT_LIBRARY_TARGET_NAME} gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} gz-sim${PROJECT_VERSION_MAJOR} gz-sim${PROJECT_VERSION_MAJOR}-gui ) +# Executable target that runs the GUI without ruby for debugging purposes. +add_executable(runGui gz.cc) +target_link_libraries(runGui PRIVATE ${gz_lib_target}) + # Create the library target gz_create_core_library(SOURCES ${sources} CXX_STANDARD 17) gz_add_get_install_prefix_impl(GET_INSTALL_PREFIX_FUNCTION gz::sim::getInstallPrefix diff --git a/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index 99c8a6baec..5ab9b0a635 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -995,6 +995,42 @@ std::unordered_set return periodicComponents; } +///////////////////////////////////////////////// +void EntityComponentManager::UpdatePeriodicChangeCache( + std::unordered_map> &_changes) const +{ + // Get all changes + for (const auto &[componentType, entities] : + this->dataPtr->periodicChangedComponents) + { + _changes[componentType].insert( + entities.begin(), entities.end()); + } + + // Get all removed components + for (const auto &[entity, components] : + this->dataPtr->componentsMarkedAsRemoved) + { + for (const auto &comp : components) + { + _changes[comp].erase(entity); + } + } + + // Get all removed entities + for (const auto &entity : this->dataPtr->toRemoveEntities) { + for ( + auto components = _changes.begin(); + components != _changes.end(); components++) { + // Its ok to leave component entries empty, the serialization + // code will simply ignore it. In any case the number of components + // is limited, so this cache will never grow too large. + components->second.erase(entity); + } + } +} + ///////////////////////////////////////////////// bool EntityComponentManager::HasEntity(const Entity _entity) const { @@ -1710,6 +1746,48 @@ void EntityComponentManager::State( }); } +////////////////////////////////////////////////// +void EntityComponentManager::PeriodicStateFromCache( + msgs::SerializedStateMap &_state, + const std::unordered_map> &_cache) const +{ + for (auto &[typeId, entities] : _cache) { + // Serialize components that have changed + for (auto &entity : entities) { + // Add entity to message if it does not exist + auto entIter = _state.mutable_entities()->find(entity); + if (entIter == _state.mutable_entities()->end()) + { + msgs::SerializedEntityMap ent; + ent.set_id(entity); + (*_state.mutable_entities())[static_cast(entity)] = ent; + entIter = _state.mutable_entities()->find(entity); + } + + // Find the component in the message + auto compIter = entIter->second.mutable_components()->find(typeId); + if (compIter != entIter->second.mutable_components()->end()) + { + // If the component is present we don't need to update it. + continue; + } + + auto compIdx = this->dataPtr->componentTypeIndex[entity][typeId]; + auto &comp = this->dataPtr->componentStorage[entity][compIdx]; + + // Add the component to the message + msgs::SerializedComponent cmp; + cmp.set_type(comp->TypeId()); + std::ostringstream ostr; + comp->Serialize(ostr); + cmp.set_component(ostr.str()); + (*(entIter->second.mutable_components()))[ + static_cast(typeId)] = cmp; + } + } +} + ////////////////////////////////////////////////// void EntityComponentManager::SetState( const msgs::SerializedState &_stateMsg) diff --git a/src/EntityComponentManager_TEST.cc b/src/EntityComponentManager_TEST.cc index 485a5c272d..bc7f03ff99 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2216,6 +2216,72 @@ TEST_P(EntityComponentManagerFixture, Descendants) } } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(UpdatePeriodicChangeCache)) +{ + Entity e1 = manager.CreateEntity(); + auto c1 = manager.CreateComponent(e1, IntComponent(123)); + + std::unordered_map> changeTracker; + + // No periodic changes keep cache empty. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 0u); + + // Create a periodic change. + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); + + // 1 periodic change, should be reflected in cache. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u); + + manager.RunSetAllComponentsUnchanged(); + + // Has periodic change. Cache should be full. + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].count(e1), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].size(), 1u); + + // Serialize state + msgs::SerializedStateMap state; + manager.PeriodicStateFromCache(state, changeTracker); + EXPECT_EQ(state.entities().size(), 1u); + EXPECT_EQ( + state.entities().find(e1)->second.components().size(), 1u); + EXPECT_NE(state.entities().find(e1)->second + .components().find(c1->TypeId()), + state.entities().find(e1)->second.components().end()); + + // Component removed cache should be updated. + manager.RemoveComponent(e1); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker.size(), 1u); + EXPECT_EQ(changeTracker[c1->TypeId()].size(), 0u); + + manager.RunSetAllComponentsUnchanged(); + + // Add another component to the entity + auto c2 = manager.CreateComponent(e1, IntComponent(123)); + manager.UpdatePeriodicChangeCache(changeTracker); + + // Cache does not track additions, only PeriodicChanges + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u); + + // Track change + manager.SetChanged(e1, c1->TypeId(), ComponentState::PeriodicChange); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 1u); + + // Entity removed cache should be updated. + manager.RequestRemoveEntity(e1); + manager.UpdatePeriodicChangeCache(changeTracker); + EXPECT_EQ(changeTracker[c2->TypeId()].size(), 0u); +} + ////////////////////////////////////////////////// TEST_P(EntityComponentManagerFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(SetChanged)) diff --git a/src/Server.cc b/src/Server.cc index 28a3d01725..6f1eb37390 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -160,6 +160,14 @@ Server::Server(const ServerConfig &_config) return; } + if (this->dataPtr->sdfRoot.WorldCount() == 0) + { + gzerr << "SDF file doesn't contain a world. " << + "If you wish to spawn a model, use the ResourceSpawner GUI plugin " << + "or the 'world//create' service.\n"; + return; + } + // Add record plugin if (_config.UseLogRecord()) { diff --git a/src/TestFixture.cc b/src/TestFixture.cc index 34626f5c04..70b3959760 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -210,5 +210,12 @@ TestFixture &TestFixture::OnPostUpdate(std::function TestFixture::Server() const { + if (!this->dataPtr->finalized) + { + gzwarn << "Fixture has not been finalized, any functions you attempted" + << "to hook into will not be run. It is recommended to call Finalize()" + << "before accessing the server." + << std::endl; + } return this->dataPtr->server; } diff --git a/src/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/gz.cc b/src/gz.cc index cd71e586bf..2d16b52789 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -452,3 +452,8 @@ extern "C" int runGui(const char *_guiConfig, const char *_file, int _waitGui, return gz::sim::gui::runGui( argc, argv, _guiConfig, _file, _waitGui, _renderEngine); } + +int main(int argc, char* argv[]) +{ + return sim::gui::runGui(argc, argv, nullptr); +} diff --git a/src/rendering/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/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index bdda4d04b5..98740bb721 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -151,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")) { @@ -166,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); @@ -185,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 d78d777c44..7cb97985f5 100644 --- a/src/systems/detachable_joint/DetachableJoint.cc +++ b/src/systems/detachable_joint/DetachableJoint.cc @@ -96,15 +96,115 @@ void DetachableJoint::Configure(const Entity &_entity, } // Setup detach topic - std::vector topics; - if (_sdf->HasElement("topic")) + std::vector detachTopics; + if (_sdf->HasElement("detach_topic")) { - topics.push_back(_sdf->Get("topic")); + detachTopics.push_back(_sdf->Get("detach_topic")); } - topics.push_back("/model/" + this->model.Name(_ecm) + + detachTopics.push_back("/model/" + this->model.Name(_ecm) + "/detachable_joint/detach"); - this->topic = validTopic(topics); + if (_sdf->HasElement("topic")) + { + if (_sdf->HasElement("detach_topic")) + { + if (_sdf->Get("topic") != + _sdf->Get("detach_topic")) + { + gzerr << " and tags have different contents. " + "Please verify the correct string and use ." + << std::endl; + } + else + { + gzdbg << "Ignoring tag and using tag." + << std::endl; + } + } + else + { + detachTopics.insert(detachTopics.begin(), + _sdf->Get("topic")); + } + } + + this->detachTopic = validTopic(detachTopics); + if (this->detachTopic.empty()) + { + gzerr << "No valid detach topics for DetachableJoint could be found.\n"; + return; + } + gzdbg << "Detach topic is: " << this->detachTopic << std::endl; + + // Setup subscriber for detach topic + this->node.Subscribe( + this->detachTopic, &DetachableJoint::OnDetachRequest, this); + + gzdbg << "DetachableJoint subscribing to messages on " + << "[" << this->detachTopic << "]" << std::endl; + + // Setup attach topic + std::vector attachTopics; + if (_sdf->HasElement("attach_topic")) + { + attachTopics.push_back(_sdf->Get("attach_topic")); + } + attachTopics.push_back("/model/" + this->model.Name(_ecm) + + "/detachable_joint/attach"); + this->attachTopic = validTopic(attachTopics); + if (this->attachTopic.empty()) + { + gzerr << "No valid attach topics for DetachableJoint could be found.\n"; + return; + } + gzdbg << "Attach topic is: " << this->attachTopic << std::endl; + + // Setup subscriber for attach topic + auto msgCb = std::function( + [this](const auto &) + { + if (this->isAttached){ + gzdbg << "Already attached" << std::endl; + return; + } + this->attachRequested = true; + }); + + if (!this->node.Subscribe(this->attachTopic, msgCb)) + { + gzerr << "Subscriber could not be created for [attach] topic.\n"; + return; + } + + // Setup output topic + std::vector outputTopics; + if (_sdf->HasElement("output_topic")) + { + outputTopics.push_back(_sdf->Get("output_topic")); + } + + outputTopics.push_back("/model/" + this->childModelName + + "/detachable_joint/state"); + + this->outputTopic = validTopic(outputTopics); + if (this->outputTopic.empty()) + { + gzerr << "No valid output topics for DetachableJoint could be found.\n"; + return; + } + gzdbg << "Output topic is: " << this->outputTopic << std::endl; + + // Setup publisher for output topic + this->outputPub = this->node.Advertise( + this->outputTopic); + if (!this->outputPub) + { + gzerr << "Error advertising topic [" << this->outputTopic << "]" + << std::endl; + return; + } + + // Supress Child Warning this->suppressChildWarning = _sdf->Get("suppress_child_warning", this->suppressChildWarning) .first; @@ -118,8 +218,13 @@ void DetachableJoint::PreUpdate( EntityComponentManager &_ecm) { GZ_PROFILE("DetachableJoint::PreUpdate"); - if (this->validConfig && !this->initialized) + // only allow attaching if child entity is detached + if (this->validConfig && !this->isAttached) { + // return if attach is not requested. + if (!this->attachRequested){ + return; + } // Look for the child model and link Entity modelEntity{kNullEntity}; @@ -148,14 +253,11 @@ void DetachableJoint::PreUpdate( this->detachableJointEntity, components::DetachableJoint({this->parentLinkEntity, this->childLinkEntity, "fixed"})); - - this->node.Subscribe( - this->topic, &DetachableJoint::OnDetachRequest, this); - - gzmsg << "DetachableJoint subscribing to messages on " - << "[" << this->topic << "]" << std::endl; - - this->initialized = true; + this->attachRequested = false; + this->isAttached = true; + this->PublishJointState(this->isAttached); + gzdbg << "Attaching entity: " << this->detachableJointEntity + << std::endl; } else { @@ -170,7 +272,8 @@ void DetachableJoint::PreUpdate( } } - if (this->initialized) + // only allow detaching if child entity is attached + if (this->isAttached) { if (this->detachRequested && (kNullEntity != this->detachableJointEntity)) { @@ -179,13 +282,34 @@ void DetachableJoint::PreUpdate( _ecm.RequestRemoveEntity(this->detachableJointEntity); this->detachableJointEntity = kNullEntity; this->detachRequested = false; + this->isAttached = false; + this->PublishJointState(this->isAttached); } } } +////////////////////////////////////////////////// +void DetachableJoint::PublishJointState(bool attached) +{ + msgs::StringMsg detachedStateMsg; + if (attached) + { + detachedStateMsg.set_data("attached"); + } + else + { + detachedStateMsg.set_data("detached"); + } + this->outputPub.Publish(detachedStateMsg); +} + ////////////////////////////////////////////////// void DetachableJoint::OnDetachRequest(const msgs::Empty &) { + if (!this->isAttached){ + gzdbg << "Already detached" << std::endl; + return; + } this->detachRequested = true; } diff --git a/src/systems/detachable_joint/DetachableJoint.hh b/src/systems/detachable_joint/DetachableJoint.hh index 310c4bac1b..e6cb491285 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -36,7 +36,9 @@ inline namespace GZ_SIM_VERSION_NAMESPACE { namespace systems { /// \brief A system that initially attaches two models via a fixed joint and - /// allows for the models to get detached during simulation via a topic. + /// allows for the models to get detached during simulation via a topic. A + /// model can be re-attached during simulation via a topic. The status of the + /// detached state can be monitored via a topic as well. /// /// Parameters: /// @@ -48,7 +50,21 @@ namespace systems /// - ``: Name of the link in the child model to be used in /// creating a fixed joint with a link in the parent model. /// - /// - `` (optional): Topic name to be used for detaching connections + /// - `` (optional): Topic name to be used for detaching connections. + /// Using is preferred. + /// + /// - `` (optional): Topic name to be used for detaching + /// connections. If multiple detachable plugin is used in one model, + /// `detach_topic` is REQUIRED to detach child models individually. + /// + /// - `` (optional): Topic name to be used for attaching + /// connections. If multiple detachable plugin is used in one model, + /// `attach_topic` is REQUIRED to attach child models individually. + /// + /// - `` (optional): Topic name to be used for publishing + /// the state of the detachment. If multiple detachable plugin is used in + /// one model, `output_topic` is REQUIRED to publish child models state + /// individually. /// /// - `` (optional): If true, the system /// will not print a warning message if a child model does not exist yet. @@ -73,6 +89,15 @@ namespace systems const gz::sim::UpdateInfo &_info, gz::sim::EntityComponentManager &_ecm) final; + /// \brief Gazebo communication node. + private: transport::Node node; + + /// \brief A publisher to send state of the detachment + private: transport::Node::Publisher outputPub; + + /// \brief Helper function to publish the state of the detachment + private: void PublishJointState(bool attached); + /// \brief Callback for detach request topic private: void OnDetachRequest(const msgs::Empty &_msg); @@ -86,7 +111,13 @@ namespace systems private: std::string childLinkName; /// \brief Topic to be used for detaching connections - private: std::string topic; + private: std::string detachTopic; + + /// \brief Topic to be used for re-attaching connections + private: std::string attachTopic; + + /// \brief Topic to be used for publishing detached state + private: std::string outputTopic; /// \brief Whether to suppress warning about missing child model. private: bool suppressChildWarning{false}; @@ -103,14 +134,15 @@ namespace systems /// \brief Whether detachment has been requested private: std::atomic detachRequested{false}; - /// \brief Gazebo communication node. - public: transport::Node node; + /// \brief Whether attachment has been requested + private: std::atomic attachRequested{true}; + + /// \brief Whether child entity is attached + private: std::atomic isAttached{false}; /// \brief Whether all parameters are valid and the system can proceed private: bool validConfig{false}; - /// \brief Whether the system has been initialized - private: bool initialized{false}; }; } } diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 0d0d42d161..db41071a6f 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -70,6 +70,9 @@ class gz::sim::systems::JointPositionControllerPrivate /// \brief mutex to protect joint commands public: std::mutex jointCmdMutex; + /// \brief Is the maximum PID gain set. + public: bool isMaxSet {false}; + /// \brief Model interface public: Model model{kNullEntity}; @@ -175,6 +178,7 @@ void JointPositionController::Configure(const Entity &_entity, if (_sdf->HasElement("cmd_max")) { cmdMax = _sdf->Get("cmd_max"); + this->dataPtr->isMaxSet = true; } if (_sdf->HasElement("cmd_min")) { @@ -430,14 +434,14 @@ void JointPositionController::PreUpdate( auto maxMovement = this->dataPtr->posPid.CmdMax() * dt; // Limit the maximum change to maxMovement - if (abs(error) > maxMovement) + if (abs(error) > maxMovement && this->dataPtr->isMaxSet) { targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() : -this->dataPtr->posPid.CmdMax(); } else { - targetVel = -error; + targetVel = - error / dt; } for (Entity joint : this->dataPtr->jointEntities) { diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index a2f5fd5ead..b9a4efce52 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -24,6 +24,7 @@ #include #include #include +#include #include #include @@ -268,8 +269,15 @@ class gz::sim::systems::SceneBroadcasterPrivate /// \brief Flag used to indicate if periodic changes need to be published /// This is currently only used in playback mode. public: bool pubPeriodicChanges{false}; + + /// \brief Stores a cache of components that are changed. (This prevents + /// dropping of periodic change components which may not be updated + /// frequently enough) + public: std::unordered_map> changedComponents; }; + ////////////////////////////////////////////////// SceneBroadcaster::SceneBroadcaster() : System(), dataPtr(std::make_unique()) @@ -352,6 +360,9 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // removed entities are removed from the scene graph for the next update cycle this->dataPtr->SceneGraphRemoveEntities(_manager); + // Iterate through entities and their changes to cache them. + _manager.UpdatePeriodicChangeCache(this->dataPtr->changedComponents); + // Publish state only if there are subscribers and // * throttle rate to 60 Hz // * also publish off-rate if there are change events: @@ -394,15 +405,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, else if (!_info.paused) { GZ_PROFILE("SceneBroadcast::PostUpdate UpdateState"); - - if (_manager.HasPeriodicComponentChanges()) - { - auto periodicComponents = _manager.ComponentTypesWithPeriodicChanges(); - _manager.State(*this->dataPtr->stepMsg.mutable_state(), - {}, periodicComponents); - this->dataPtr->pubPeriodicChanges = false; - } - else + if (!_manager.HasPeriodicComponentChanges()) { // log files may be recorded at lower rate than sim time step. So in // playback mode, the scene broadcaster may not see any periodic @@ -427,6 +430,12 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, // we may be able to remove this in the future and update tests this->dataPtr->stepMsg.mutable_state(); } + + // Apply changes that were caught by the periodic state tracker and then + // clear the change tracker. + _manager.PeriodicStateFromCache(*this->dataPtr->stepMsg.mutable_state(), + this->dataPtr->changedComponents); + this->dataPtr->changedComponents.clear(); } // Full state on demand diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index a346076f6d..e9e43b862f 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -17,6 +17,8 @@ #include "Sensors.hh" +#include +#include #include #include #include diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 59eae5c5c4..fedae18b24 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -101,6 +101,7 @@ set(tests_needing_display depth_camera.cc distortion_camera.cc gpu_lidar.cc + markers.cc mesh_uri.cc optical_tactile_plugin.cc reset_sensors.cc @@ -111,6 +112,7 @@ set(tests_needing_display shader_param_system.cc thermal_sensor_system.cc thermal_system.cc + triggered_camera.cc wide_angle_camera.cc ) @@ -149,6 +151,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..13290c5b99 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -213,3 +213,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 8cd5d802df..08ce481548 100644 --- a/test/integration/sensors_system_update_rate.cc +++ b/test/integration/sensors_system_update_rate.cc @@ -77,8 +77,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/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/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/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 ```