From 2a125148efd13fc611ba243b2c8717cd7042d084 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 2 May 2023 23:14:32 -0500 Subject: [PATCH 01/18] Speed up Resource Spawner load time by fetching model list asynchronously (#1962) The main reason that the Resource Spawner took a long time to load is that it tried to fetch the list of all available models on Fuel instead of just the selected owner. And it did that while blocking the Qt thread, so the user was unable to interact with the GUI while the model list was being fetched. The approach I've taken is to only fetch the list of models for the default owner ("openrobotics") and allow users to add/remove any owner they want. The fetching is also done in a separate thread so as to not block the GUI. --------- Signed-off-by: Addisu Z. Taddese --- src/CMakeLists.txt | 6 +- .../resource_spawner/ResourceSpawner.cc | 294 ++++++++++++------ .../resource_spawner/ResourceSpawner.hh | 53 +++- .../resource_spawner/ResourceSpawner.qml | 216 ++++++++----- src/gz.cc | 5 + 5 files changed, 398 insertions(+), 176 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 6c52f7d832..d6c94cccfe 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -37,13 +37,17 @@ ign_add_component(ign cmd/ModelCommandAPI.cc GET_TARGET_NAME ign_lib_target) target_link_libraries(${ign_lib_target} - PRIVATE + PUBLIC ${PROJECT_LIBRARY_TARGET_NAME} ignition-common${IGN_COMMON_VER}::ignition-common${IGN_COMMON_VER} ignition-gazebo${PROJECT_VERSION_MAJOR} ignition-gazebo${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 ${ign_lib_target}) + set (sources Barrier.cc Conversions.cc diff --git a/src/gui/plugins/resource_spawner/ResourceSpawner.cc b/src/gui/plugins/resource_spawner/ResourceSpawner.cc index 1db342258a..8d89e3aff2 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.cc +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.cc @@ -22,6 +22,7 @@ #include #include +#include #include #include @@ -40,6 +41,9 @@ #include "gz/sim/gui/GuiEvents.hh" + +Q_DECLARE_METATYPE(ignition::gazebo::Resource) + namespace ignition::gazebo { class ResourceSpawnerPrivate @@ -70,9 +74,34 @@ namespace ignition::gazebo /// \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 ignition; using namespace ignition::gazebo; @@ -86,15 +115,27 @@ void PathModel::AddPath(const std::string &_path) { IGN_PROFILE_THREAD_NAME("Qt thread"); IGN_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) +{ + IGN_PROFILE_THREAD_NAME("Qt thread"); + IGN_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; + } + } } ///////////////////////////////////////////////// @@ -114,14 +155,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(); } ///////////////////////////////////////////////// @@ -132,13 +168,10 @@ void ResourceModel::AddResources(std::vector &_resources) } ///////////////////////////////////////////////// -void ResourceModel::AddResource(Resource &_resource) +void ResourceModel::AddResource(const Resource &_resource) { IGN_PROFILE_THREAD_NAME("Qt thread"); IGN_PROFILE("GridModel::AddResource"); - QStandardItem *parentItem{nullptr}; - - parentItem = this->invisibleRootItem(); auto resource = new QStandardItem(QString::fromStdString(_resource.name)); resource->setData(_resource.isFuel, @@ -166,8 +199,9 @@ void ResourceModel::AddResource(Resource &_resource) this->roleNames().key("index")); this->gridIndex++; } + emit sizeChanged(); - parentItem->appendRow(resource); + this->appendRow(resource); } ///////////////////////////////////////////////// @@ -209,6 +243,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( @@ -217,10 +252,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, @@ -330,7 +400,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); } @@ -549,85 +619,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); - } - } - ignmsg << "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); - } - ignmsg << "Fuel resources loaded.\n"; - }); - t.detach(); + this->dataPtr->ownerModel.AddPath(kDefaultOwner); + RunFetchResourceListThread(kDefaultOwner); } ///////////////////////////////////////////////// @@ -653,6 +647,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(ignition::gazebo::Resource, resource)); + } + } + if (counter == 0) + { + QString errorMsg = QString("No resources found for %1") + .arg(QString::fromStdString(owner)); + emit resourceSpawnerError(errorMsg); + } + }); +} + // Register this plugin IGNITION_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 6a378cbe81..0cf31016f8 100644 --- a/src/gui/plugins/resource_spawner/ResourceSpawner.hh +++ b/src/gui/plugins/resource_spawner/ResourceSpawner.hh @@ -18,9 +18,12 @@ #ifndef GZ_GAZEBO_GUI_RESOURCE_SPAWNER_HH_ #define GZ_GAZEBO_GUI_RESOURCE_SPAWNER_HH_ +#include #include #include #include +#include +#include #include @@ -92,9 +95,13 @@ namespace gazebo /// \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; @@ -106,6 +113,10 @@ namespace gazebo { 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(); @@ -114,7 +125,7 @@ namespace gazebo /// \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 @@ -133,6 +144,9 @@ namespace gazebo // 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; @@ -237,6 +251,37 @@ namespace gazebo 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( + ignition::gazebo::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 91124c1ec4..5110d197b1 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -429,3 +429,8 @@ extern "C" IGNITION_GAZEBO_VISIBLE int runGui( return gz::sim::gui::runGui( argc, &argv, _guiConfig, _file, _waitGui); } + +int main(int argc, char* argv[]) +{ + return gazebo::gui::runGui(argc, argv, nullptr); +} From a6be7302e43f197ce731f81bc685e24923345c0e Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Tue, 9 May 2023 09:14:25 -0700 Subject: [PATCH 02/18] Small fixes to gz headers (#1985) * Include gz/msgs.hh instead of ignition * Install components.hh to gz/sim Signed-off-by: Steve Peters --- include/gz/sim/components/CMakeLists.txt | 2 +- include/gz/sim/gui/TmpIface.hh | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/gz/sim/components/CMakeLists.txt b/include/gz/sim/components/CMakeLists.txt index 83f18ce69d..100786be0e 100644 --- a/include/gz/sim/components/CMakeLists.txt +++ b/include/gz/sim/components/CMakeLists.txt @@ -13,5 +13,5 @@ configure_file( install( FILES ${CMAKE_CURRENT_BINARY_DIR}/components.hh - DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}/ignition/${IGN_DESIGNATION} + DESTINATION ${IGN_INCLUDE_INSTALL_DIR_FULL}/gz/sim ) diff --git a/include/gz/sim/gui/TmpIface.hh b/include/gz/sim/gui/TmpIface.hh index 8709d3036d..ed54a7597b 100644 --- a/include/gz/sim/gui/TmpIface.hh +++ b/include/gz/sim/gui/TmpIface.hh @@ -21,7 +21,7 @@ #include #endif -#include +#include #include #include "gz/sim/Export.hh" From ea3415862819c0c7f447d4dba653ce8f10b1c66c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 10 May 2023 08:30:53 -0500 Subject: [PATCH 03/18] Prepare for 3.15.0 Release (#1984) Signed-off-by: Addisu Z. Taddese Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 2 +- Changelog.md | 64 ++++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 65 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 5c5abddeba..ae902446a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo3 VERSION 3.14.0) +project(ignition-gazebo3 VERSION 3.15.0) set (GZ_DISTRIBUTION "Citadel") #============================================================================ diff --git a/Changelog.md b/Changelog.md index 4873dea3d3..a994ba13b2 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,69 @@ ## 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.14.0 (2022-08-17) 1. Change `CODEOWNERS` and maintainer to Michael From f0ef915bfa505b82cabce5af868b968825dc8c4c Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 10 May 2023 08:41:23 -0500 Subject: [PATCH 04/18] Update workflow triggers to avoid duplicates (#1988) Signed-off-by: Addisu Z. Taddese --- .github/workflows/ci.yml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 744bfcff3b..438c601c92 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: + - 'ign-gazebo3' jobs: bionic-ci: From 3dd77a2ae3eccd40d5c32f3ec7a56c3664165049 Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 23 May 2023 09:03:49 -0700 Subject: [PATCH 05/18] Enable GzWeb visualization of markers by republishing service requests on a topic (#1994) * Working on marker topic Signed-off-by: Nate Koenig * Fixed build Signed-off-by: Nate Koenig * Fixed test Signed-off-by: Nate Koenig * Cleanup Signed-off-by: Nate Koenig * codecheck Signed-off-by: Nate Koenig --------- Signed-off-by: Nate Koenig --- include/gz/sim/rendering/MarkerManager.hh | 1 + src/rendering/MarkerManager.cc | 7 ++ test/integration/CMakeLists.txt | 2 + test/integration/markers.cc | 114 ++++++++++++++++++++++ 4 files changed, 124 insertions(+) create mode 100644 test/integration/markers.cc diff --git a/include/gz/sim/rendering/MarkerManager.hh b/include/gz/sim/rendering/MarkerManager.hh index 5139505772..26c4b0d716 100644 --- a/include/gz/sim/rendering/MarkerManager.hh +++ b/include/gz/sim/rendering/MarkerManager.hh @@ -20,6 +20,7 @@ #include #include +#include #include #include "gz/rendering/RenderTypes.hh" diff --git a/src/rendering/MarkerManager.cc b/src/rendering/MarkerManager.cc index 3e06e7c935..7b61b267f2 100644 --- a/src/rendering/MarkerManager.cc +++ b/src/rendering/MarkerManager.cc @@ -119,6 +119,9 @@ class ignition::gazebo::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; }; ///////////////////////////////////////////////// @@ -189,6 +192,9 @@ bool MarkerManager::Init(const rendering::ScenePtr &_scene) << "_array service.\n"; } + this->dataPtr->markerPub = + this->dataPtr->node.Advertise(this->dataPtr->topicName); + return true; } @@ -215,6 +221,7 @@ void MarkerManagerPrivate::Update() markerIter != this->markerMsgs.end();) { this->ProcessMarkerMsg(*markerIter); + this->markerPub.Publish(*markerIter); this->markerMsgs.erase(markerIter++); } diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index f8335cfe0c..57ed3f3723 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -61,6 +61,7 @@ set(tests_needing_display camera_video_record_system.cc depth_camera.cc gpu_lidar.cc + markers.cc rgbd_camera.cc sensors_system.cc ) @@ -86,6 +87,7 @@ ign_build_tests(TYPE INTEGRATION ${tests} LIB_DEPS ${EXTRA_TEST_LIB_DEPS} + ${PROJECT_LIBRARY_TARGET_NAME}-rendering ) # For INTEGRATION_physics_system, we need to check what version of DART is diff --git a/test/integration/markers.cc b/test/integration/markers.cc new file mode 100644 index 0000000000..c4682d0b3b --- /dev/null +++ b/test/integration/markers.cc @@ -0,0 +1,114 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include +#include +#include +#include "gz/rendering/Scene.hh" +#include +#include + +#include "gz/sim/rendering/MarkerManager.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/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 = ignition::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); +} From 568c0265b89d3d5503b434858b58188b87ac36a0 Mon Sep 17 00:00:00 2001 From: Liam Han Date: Mon, 5 Jun 2023 10:47:15 -0700 Subject: [PATCH 06/18] Allow re-attaching detached joint (#1687) Extending the feature for detachable_joint plugin to support re-attaching the joint again. By publishing to a topic, the child link can be easily reattached with a fixed joint to its parent link. Signed-off-by: Liam Han Signed-off-by: Liam Han Co-authored-by: Addisu Z. Taddese --- examples/worlds/detachable_joint.sdf | 33 +- examples/worlds/triggered_publisher.sdf | 4 +- .../detachable_joint/DetachableJoint.cc | 154 +++++++- .../detachable_joint/DetachableJoint.hh | 46 ++- test/integration/detachable_joint.cc | 142 ++++++++ test/worlds/detachable_joint.sdf | 334 +++++++++++++++++- tutorials/detachable_joints.md | 31 +- tutorials/triggered_publisher.md | 4 +- 8 files changed, 710 insertions(+), 38 deletions(-) diff --git a/examples/worlds/detachable_joint.sdf b/examples/worlds/detachable_joint.sdf index 1aea0d3a24..2ccd32e622 100644 --- a/examples/worlds/detachable_joint.sdf +++ b/examples/worlds/detachable_joint.sdf @@ -13,6 +13,18 @@ ign topic -t "/B1/detach" -m ignition.msgs.Empty -p "unused: true" ign topic -t "/B2/detach" -m ignition.msgs.Empty -p "unused: true" ign topic -t "/B3/detach" -m ignition.msgs.Empty -p "unused: true" + + To re-attach breadcrumbs + + ign topic -t "/B1/attach" -m ignition.msgs.Empty -p "unused: true" + ign topic -t "/B2/attach" -m ignition.msgs.Empty -p "unused: true" + ign topic -t "/B3/attach" -m ignition.msgs.Empty -p "unused: true" + + To monitor the state of each breadcrumbs + + ign topic -e -t /B1/state + ign topic -e -t /B2/state + ign topic -e -t /B3/state --> @@ -353,23 +365,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 dbc2a43022..4d18bc4fed 100644 --- a/examples/worlds/triggered_publisher.sdf +++ b/examples/worlds/triggered_publisher.sdf @@ -391,13 +391,13 @@ start falling. body box1 box_body - /box1/detach + /box1/detach body box2 box_body - /box2/detach + /box2/detach diff --git a/src/systems/detachable_joint/DetachableJoint.cc b/src/systems/detachable_joint/DetachableJoint.cc index 15885a14f0..f41a26d93d 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")) + { + ignerr << " and tags have different contents. " + "Please verify the correct string and use ." + << std::endl; + } + else + { + igndbg << "Ignoring tag and using tag." + << std::endl; + } + } + else + { + detachTopics.insert(detachTopics.begin(), + _sdf->Get("topic")); + } + } + + this->detachTopic = validTopic(detachTopics); + if (this->detachTopic.empty()) + { + ignerr << "No valid detach topics for DetachableJoint could be found.\n"; + return; + } + igndbg << "Detach topic is: " << this->detachTopic << std::endl; + + // Setup subscriber for detach topic + this->node.Subscribe( + this->detachTopic, &DetachableJoint::OnDetachRequest, this); + + igndbg << "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()) + { + ignerr << "No valid attach topics for DetachableJoint could be found.\n"; + return; + } + igndbg << "Attach topic is: " << this->attachTopic << std::endl; + + // Setup subscriber for attach topic + auto msgCb = std::function( + [this](const auto &) + { + if (this->isAttached){ + igndbg << "Already attached" << std::endl; + return; + } + this->attachRequested = true; + }); + + if (!this->node.Subscribe(this->attachTopic, msgCb)) + { + ignerr << "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()) + { + ignerr << "No valid output topics for DetachableJoint could be found.\n"; + return; + } + igndbg << "Output topic is: " << this->outputTopic << std::endl; + + // Setup publisher for output topic + this->outputPub = this->node.Advertise( + this->outputTopic); + if (!this->outputPub) + { + ignerr << "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) { IGN_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); - - ignmsg << "DetachableJoint subscribing to messages on " - << "[" << this->topic << "]" << std::endl; - - this->initialized = true; + this->attachRequested = false; + this->isAttached = true; + this->PublishJointState(this->isAttached); + igndbg << "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) +{ + ignition::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){ + igndbg << "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 57fdfca98b..f0f800b4e5 100644 --- a/src/systems/detachable_joint/DetachableJoint.hh +++ b/src/systems/detachable_joint/DetachableJoint.hh @@ -36,7 +36,9 @@ inline namespace IGNITION_GAZEBO_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 Ignition 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/test/integration/detachable_joint.cc b/test/integration/detachable_joint.cc index 0c3b19f9e5..f80240546b 100644 --- a/test/integration/detachable_joint.cc +++ b/test/integration/detachable_joint.cc @@ -213,3 +213,145 @@ TEST_F(DetachableJointTest, IGN_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, IGN_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 gazebo::UpdateInfo &, + const gazebo::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 + igndbg << "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()); + igndbg << "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/worlds/detachable_joint.sdf b/test/worlds/detachable_joint.sdf index afe0a7efac..a9b73307aa 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/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 61b4844606..0b2b6374fb 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 ``` From c0a7428adb9c0dccce30fc5d5163cad6ec762990 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 6 Jun 2023 01:08:54 -0700 Subject: [PATCH 07/18] Port record topic fix (#2004) Signed-off-by: Ian Chen --- src/ServerPrivate.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/ServerPrivate.cc b/src/ServerPrivate.cc index 7906307a2b..f38dd9d025 100644 --- a/src/ServerPrivate.cc +++ b/src/ServerPrivate.cc @@ -222,9 +222,8 @@ void ServerPrivate::AddRecordPlugin(const ServerConfig &_config) { auto topic = recordTopicElem->Get(); sdfRecordTopics.push_back(topic); + recordTopicElem = recordTopicElem->GetNextElement(); } - - recordTopicElem = recordTopicElem->GetNextElement(); } // Remove the plugin, which will be added back in by ServerConfig. From 1038a355866eb1003c8b370b01a673d431c51052 Mon Sep 17 00:00:00 2001 From: Henrique Barros Oliveira Date: Mon, 12 Jun 2023 22:11:57 +0200 Subject: [PATCH 08/18] Print an error message when trying to load SDF files that don't contain a `` (#1998) Signed-off-by: Henrique-BO --- src/Server.cc | 8 ++++++++ src/Server_TEST.cc | 39 +++++++++++++++++++++++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/src/Server.cc b/src/Server.cc index deb622cf5d..51ad4b2e0c 100644 --- a/src/Server.cc +++ b/src/Server.cc @@ -182,6 +182,14 @@ Server::Server(const ServerConfig &_config) return; } + if (this->dataPtr->sdfRoot.WorldCount() == 0) + { + ignerr << "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/Server_TEST.cc b/src/Server_TEST.cc index a2ff63ae55..4a16d723db 100644 --- a/src/Server_TEST.cc +++ b/src/Server_TEST.cc @@ -1088,6 +1088,45 @@ TEST_P(ServerFixture, AddResourcePaths) } } +///////////////////////////////////////////////// +TEST_P(ServerFixture, SdfWithoutWorld) +{ + // Initialize logging + std::string logBasePath = common::joinPaths(PROJECT_BINARY_PATH, "tmp"); + common::setenv(IGN_HOMEDIR, logBasePath); + std::string path = common::uuid(); + ignLogInit(path, "test.log"); + + // Start server with model SDF file + ServerConfig serverConfig; + serverConfig.SetSdfFile(common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "models", "sphere", "model.sdf")); + + gz::sim::Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + // Check error message in log file + std::string logFullPath = common::joinPaths(logBasePath, path, "test.log"); + std::ifstream ifs(logFullPath.c_str(), std::ios::in); + bool errFound = false; + while ((!errFound) && (!ifs.eof())) + { + std::string line; + std::getline(ifs, line); + std::string errString = "SDF file doesn't contain a world. "; + errString += "If you wish to spawn a model, "; + errString += "use the ResourceSpawner GUI plugin "; + errString += "or the 'world//create' service."; + errFound = (line.find(errString) != std::string::npos); + } + EXPECT_TRUE(errFound); + + // Stop logging + ignLogClose(); + common::removeAll(logBasePath); +} + // Run multiple times. We want to make sure that static globals don't cause // problems. INSTANTIATE_TEST_SUITE_P(ServerRepeat, ServerFixture, ::testing::Range(1, 2)); From 090402e249733a847c02a3199871872d9c9fc95d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Crist=C3=B3bal=20Arroyo?= Date: Mon, 12 Jun 2023 16:10:56 -0500 Subject: [PATCH 09/18] Disable pybind11 on Windows by default (#2005) Signed-off-by: Crola1702 --- CMakeLists.txt | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8d04ff2326..eef08b148e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,6 +39,17 @@ else() set (EXTRA_TEST_LIB_DEPS) endif() +# 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" + ${skip_pybind11_default_value}) + include(test/find_dri.cmake) FindDRI() @@ -196,14 +207,17 @@ else() set(PYBIND11_PYTHON_VERSION 3) find_package(Python3 QUIET COMPONENTS Interpreter Development) - find_package(pybind11 2.2 QUIET) - - if (${pybind11_FOUND}) - message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") - else() - IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") - message (STATUS "Searching for pybind11 - not found.") - endif() + + if (NOT SKIP_PYBIND11) + find_package(pybind11 2.2 QUIET) + + if (${pybind11_FOUND}) + message (STATUS "Searching for pybind11 - found version ${pybind11_VERSION}.") + else() + IGN_BUILD_WARNING("pybind11 is missing: Python interfaces are disabled.") + message (STATUS "Searching for pybind11 - not found.") + endif() + endif() endif() # Plugin install dirs set(IGNITION_GAZEBO_PLUGIN_INSTALL_DIR @@ -225,7 +239,7 @@ add_subdirectory(examples) #============================================================================ ign_create_packages() -if (${pybind11_FOUND}) +if (pybind11_FOUND) add_subdirectory(python) endif() #============================================================================ From dfb70396bdb426f68f3858ba40bbfb0fe73408be Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 14 Jun 2023 07:20:11 +0200 Subject: [PATCH 10/18] Protobuf: Do not require version 3 do support Protobuf 4.23.2 (23.2) (#2006) Signed-off-by: Silvio Traversaro * TriggeredPublisher: don't catch FatalException It has been removed from recent versions of protobuf. Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 2 -- src/systems/triggered_publisher/TriggeredPublisher.cc | 9 +-------- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index eef08b148e..6453a20d7e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -187,9 +187,7 @@ set(IGN_UTILS_VER ${ignition-utils1_VERSION_MAJOR}) #-------------------------------------- # Find protobuf -set(REQ_PROTOBUF_VER 3) ign_find_package(IgnProtobuf - VERSION ${REQ_PROTOBUF_VER} REQUIRED COMPONENTS all PRETTY Protobuf) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index 28f106584d..b8d1d70c0f 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -843,14 +843,7 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) return std::all_of(this->matchers.begin(), this->matchers.end(), [&](const auto &_matcher) { - try - { - return _matcher->Match(_inputMsg); - } catch (const google::protobuf::FatalException &err) - { - ignerr << err.what() << std::endl; - return false; - } + return _matcher->Match(_inputMsg); }); } From e22e381135b7e38a8a547156d622beca7a3368d5 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Wed, 14 Jun 2023 07:20:11 +0200 Subject: [PATCH 11/18] Protobuf: Do not require version 3 do support Protobuf 4.23.2 (23.2) (#2006) Signed-off-by: Silvio Traversaro * TriggeredPublisher: don't catch FatalException It has been removed from recent versions of protobuf. Signed-off-by: Steve Peters Co-authored-by: Steve Peters --- CMakeLists.txt | 2 -- src/systems/triggered_publisher/TriggeredPublisher.cc | 9 +-------- 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ae902446a6..8f1a921d1d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -146,9 +146,7 @@ set(IGN_TOOLS_VER 1) #-------------------------------------- # Find protobuf -set(REQ_PROTOBUF_VER 3) ign_find_package(IgnProtobuf - VERSION ${REQ_PROTOBUF_VER} REQUIRED COMPONENTS all PRETTY Protobuf) diff --git a/src/systems/triggered_publisher/TriggeredPublisher.cc b/src/systems/triggered_publisher/TriggeredPublisher.cc index ee3b20dc86..fced273937 100644 --- a/src/systems/triggered_publisher/TriggeredPublisher.cc +++ b/src/systems/triggered_publisher/TriggeredPublisher.cc @@ -652,14 +652,7 @@ bool TriggeredPublisher::MatchInput(const transport::ProtoMsg &_inputMsg) return std::all_of(this->matchers.begin(), this->matchers.end(), [&](const auto &_matcher) { - try - { - return _matcher->Match(_inputMsg); - } catch (const google::protobuf::FatalException &err) - { - ignerr << err.what() << std::endl; - return false; - } + return _matcher->Match(_inputMsg); }); } From 56ed4b06363ea0db80ba650e1fb1e959ecb1d2f6 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 26 Jul 2023 11:05:30 +0800 Subject: [PATCH 12/18] Adds a warning if the `Server` method of a `TestFixture` is called before `Finalize` (#2047) * Adds a warning if the `Server` method of a `TestFixture` is called before `Finalize This commit adds a warning if someone were to call `Server()` before `Finalize`. The rationale for this is because I made the mistake of not calling `Finalize` ina program I was writing. It took me some time to track down the root cause of the problem. I figure if we warn users it should be immediately obvious. Signed-off-by: Arjo Chakravarty * switch to gzwarn over ignwarn Signed-off-by: Arjo Chakravarty * ignwarn cause citadel :( Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty --- src/TestFixture.cc | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/TestFixture.cc b/src/TestFixture.cc index 7e05455ed2..0b6e90a258 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -210,6 +210,13 @@ TestFixture &TestFixture::OnPostUpdate(std::function TestFixture::Server() const { + if (!this->dataPtr->finalized) + { + ignwarn << "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; } From 8a4bc3082b3f536ef6e53d137724794073a7b5bc Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Sun, 30 Jul 2023 23:57:52 -0700 Subject: [PATCH 13/18] Backport sensors system threading optimization changes (#2058) Signed-off-by: Ian Chen --- include/gz/sim/EventManager.hh | 25 ++ src/EventManager_TEST.cc | 10 + src/systems/sensors/Sensors.cc | 277 +++++++++++----- test/integration/CMakeLists.txt | 1 + .../integration/sensors_system_update_rate.cc | 311 ++++++++++++++++++ test/worlds/sensor.sdf | 9 +- 6 files changed, 544 insertions(+), 89 deletions(-) create mode 100644 test/integration/sensors_system_update_rate.cc diff --git a/include/gz/sim/EventManager.hh b/include/gz/sim/EventManager.hh index d0e83e40cb..d4a7d302cb 100644 --- a/include/gz/sim/EventManager.hh +++ b/include/gz/sim/EventManager.hh @@ -118,6 +118,31 @@ namespace ignition } } + /// \brief Get connection count for a particular event + /// Connection count for the event + public: template + unsigned int + ConnectionCount() + { + if (this->events.find(typeid(E)) == this->events.end()) + { + return 0u; + } + + E *eventPtr = dynamic_cast(this->events[typeid(E)].get()); + // All values in the map should be derived from Event, + // so this shouldn't be an issue, but it doesn't hurt to check. + if (eventPtr != nullptr) + { + return eventPtr->ConnectionCount(); + } + else + { + ignerr << "Failed to get connection count for event: " + << typeid(E).name() << std::endl; + return 0u; + } + } /// \brief Convenience type for storing typeinfo references. private: using TypeInfoRef = std::reference_wrapper; diff --git a/src/EventManager_TEST.cc b/src/EventManager_TEST.cc index 78ef5cda90..fd0b30a95d 100644 --- a/src/EventManager_TEST.cc +++ b/src/EventManager_TEST.cc @@ -29,12 +29,17 @@ TEST(EventManager, EmitConnectTest) { EventManager eventManager; + EXPECT_EQ(0u, eventManager.ConnectionCount()); + bool paused1 = false; auto connection1 = eventManager.Connect( [&paused1](bool _paused) { paused1 = _paused; }); + + EXPECT_EQ(1u, eventManager.ConnectionCount()); + // Emitting events causes connection callbacks to be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -47,6 +52,8 @@ TEST(EventManager, EmitConnectTest) paused2 = _paused; }); + EXPECT_EQ(2u, eventManager.ConnectionCount()); + // Multiple connections should each be fired. eventManager.Emit(true); EXPECT_EQ(true, paused1); @@ -58,9 +65,12 @@ TEST(EventManager, EmitConnectTest) // Clearing the ConnectionPtr will cause it to no longer fire. connection1.reset(); + eventManager.Emit(true); EXPECT_EQ(false, paused1); EXPECT_EQ(true, paused2); + + EXPECT_EQ(1u, eventManager.ConnectionCount()); } /// Test that we are able to connect arbitrary events and signal them. diff --git a/src/systems/sensors/Sensors.cc b/src/systems/sensors/Sensors.cc index 78a6468d7f..cb20661c55 100644 --- a/src/systems/sensors/Sensors.cc +++ b/src/systems/sensors/Sensors.cc @@ -17,7 +17,8 @@ #include "Sensors.hh" -#include +#include +#include #include #include #include @@ -73,7 +74,7 @@ class ignition::gazebo::systems::SensorsPrivate public: sensors::Manager sensorManager; /// \brief used to store whether rendering objects have been created. - public: bool initialized = false; + public: std::atomic initialized { false }; /// \brief Main rendering interface public: RenderUtil renderUtil; @@ -96,7 +97,7 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Keep track of cameras, in case we need to handle stereo cameras. /// Key: Camera's parent scoped name /// Value: Pointer to camera - public: std::map cameras; + public: std::unordered_map cameras; /// \brief Maps gazebo entity to its matching sensor ID /// @@ -111,7 +112,10 @@ class ignition::gazebo::systems::SensorsPrivate public: bool doInit { false }; /// \brief Flag to signal if rendering update is needed - public: bool updateAvailable { false }; + public: std::atomic updateAvailable { false }; + + /// \brief Flag to signal if a rendering update must be done + public: std::atomic forceUpdate { false }; /// \brief Thread that rendering will occur in public: std::thread renderThread; @@ -132,14 +136,20 @@ class ignition::gazebo::systems::SensorsPrivate /// \brief Update time for the next rendering iteration public: std::chrono::steady_clock::duration updateTime; + /// \brief Next sensors update time + public: std::chrono::steady_clock::duration nextUpdateTime; + /// \brief Sensors to include in the next rendering iteration - public: std::vector activeSensors; + public: std::set activeSensors; + + /// \brief Sensors to be updated next + public: std::set sensorsToUpdate; /// \brief Mutex to protect sensorMask - public: std::mutex sensorMaskMutex; + public: std::mutex sensorsMutex; /// \brief Mask sensor updates for sensors currently being rendered - public: std::map sensorMask; /// \brief Pointer to the event manager @@ -189,6 +199,14 @@ class ignition::gazebo::systems::SensorsPrivate /// \param[in] _ecm Entity component manager public: void UpdateBatteryState(const EntityComponentManager &_ecm); + /// \brief Get the next closest sensor update time + public: std::chrono::steady_clock::duration NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime); + + /// \brief Check if any of the sensors have connections + public: bool SensorsHaveConnections(); + /// \brief Check if sensor has subscribers /// \param[in] _sensor Sensor to check /// \return True if the sensor has subscribers, false otherwise @@ -210,7 +228,7 @@ class ignition::gazebo::systems::SensorsPrivate public: std::unordered_map modelBatteryStateChanged; /// \brief A map of sensor ids to their active state - public: std::map sensorStateChanged; + public: std::unordered_map sensorStateChanged; /// \brief Disable sensors if parent model's battery is drained /// Affects sensors that are in nested models @@ -257,11 +275,16 @@ void SensorsPrivate::WaitForInit() ////////////////////////////////////////////////// void SensorsPrivate::RunOnce() { - std::unique_lock lock(this->renderMutex); - this->renderCv.wait(lock, [this]() { - return !this->running || this->updateAvailable; - }); + std::unique_lock cvLock(this->renderMutex); + this->renderCv.wait_for(cvLock, std::chrono::microseconds(1000), [this]() + { + return !this->running || this->updateAvailable; + }); + } + + if (!this->updateAvailable) + return; if (!this->running) return; @@ -269,13 +292,22 @@ void SensorsPrivate::RunOnce() if (!this->scene) return; + std::chrono::steady_clock::duration updateTimeApplied; IGN_PROFILE("SensorsPrivate::RunOnce"); { IGN_PROFILE("Update"); + std::unique_lock timeLock(this->renderMutex); this->renderUtil.Update(); + updateTimeApplied = this->updateTime; } - if (!this->activeSensors.empty()) + bool activeSensorsEmpty = true; + { + std::unique_lock lk(this->sensorsMutex); + activeSensorsEmpty = this->activeSensors.empty(); + } + + if (!activeSensorsEmpty || this->forceUpdate) { // disable sensors that are out of battery or re-enable sensors that are // being charged @@ -294,29 +326,10 @@ void SensorsPrivate::RunOnce() this->sensorStateChanged.clear(); } - this->sensorMaskMutex.lock(); - // Check the active sensors against masked sensors. - // - // The internal state of a rendering sensor is not updated until the - // rendering operation is complete, which can leave us in a position - // where the sensor is falsely indicating that an update is needed. - // - // To prevent this, add sensors that are currently being rendered to - // a mask. Sensors are removed from the mask when 90% of the update - // delta has passed, which will allow rendering to proceed. - for (const auto & sensor : this->activeSensors) - { - // 90% of update delta (1/UpdateRate()); - auto delta = std::chrono::duration_cast< std::chrono::milliseconds>( - std::chrono::duration< double >(0.9 / sensor->UpdateRate())); - this->sensorMask[sensor->Id()] = this->updateTime + delta; - } - this->sensorMaskMutex.unlock(); - { IGN_PROFILE("PreRender"); this->eventManager->Emit(); - this->scene->SetTime(this->updateTime); + this->scene->SetTime(updateTimeApplied); // Update the scene graph manually to improve performance // We only need to do this once per frame It is important to call // sensors::RenderingSensor::SetManualSceneUpdate and set it to true @@ -327,7 +340,7 @@ void SensorsPrivate::RunOnce() // disable sensors that have no subscribers to prevent doing unnecessary // work std::unordered_set tmpDisabledSensors; - this->sensorMaskMutex.lock(); + this->sensorsMutex.lock(); for (auto id : this->sensorIds) { sensors::Sensor *s = this->sensorManager.Sensor(id); @@ -338,12 +351,16 @@ void SensorsPrivate::RunOnce() tmpDisabledSensors.insert(rs); } } - this->sensorMaskMutex.unlock(); + this->sensorsMutex.unlock(); + // safety check to see if reset occurred while we're rendering + // avoid publishing outdated data if reset occurred + std::unique_lock timeLock(this->renderMutex); + if (updateTimeApplied <= this->updateTime) { // publish data IGN_PROFILE("RunOnce"); - this->sensorManager.RunOnce(this->updateTime); + this->sensorManager.RunOnce(updateTimeApplied); } // re-enble sensors @@ -366,7 +383,7 @@ void SensorsPrivate::RunOnce() } this->updateAvailable = false; - lock.unlock(); + this->forceUpdate = false; this->renderCv.notify_one(); } @@ -432,15 +449,9 @@ void Sensors::RemoveSensor(const Entity &_entity) // Locking mutex to make sure the vector is not being changed while // the rendering thread is iterating over it { - std::unique_lock lock(this->dataPtr->sensorMaskMutex); - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(idIter->second); - auto rs = dynamic_cast(s); - auto activeSensorIt = std::find(this->dataPtr->activeSensors.begin(), - this->dataPtr->activeSensors.end(), rs); - if (activeSensorIt != this->dataPtr->activeSensors.end()) - { - this->dataPtr->activeSensors.erase(activeSensorIt); - } + std::unique_lock lock(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors.erase(idIter->second); + this->dataPtr->sensorsToUpdate.erase(idIter->second); } // update cameras list @@ -549,7 +560,6 @@ void Sensors::Update(const UpdateInfo &_info, EntityComponentManager &_ecm) { IGN_PROFILE("Sensors::Update"); - std::unique_lock lock(this->dataPtr->renderMutex); if (this->dataPtr->running && this->dataPtr->initialized) { this->dataPtr->renderUtil.UpdateECM(_info, _ecm); @@ -561,7 +571,6 @@ void Sensors::PostUpdate(const UpdateInfo &_info, const EntityComponentManager &_ecm) { IGN_PROFILE("Sensors::PostUpdate"); - // \TODO(anyone) Support rewind if (_info.dt < std::chrono::steady_clock::duration::zero()) { @@ -571,9 +580,9 @@ void Sensors::PostUpdate(const UpdateInfo &_info, } { - std::unique_lock lock(this->dataPtr->renderMutex); if (!this->dataPtr->initialized && - (_ecm.HasComponentType(components::Camera::typeId) || + (this->dataPtr->forceUpdate || + _ecm.HasComponentType(components::Camera::typeId) || _ecm.HasComponentType(components::DepthCamera::typeId) || _ecm.HasComponentType(components::GpuLidar::typeId) || _ecm.HasComponentType(components::RgbdCamera::typeId) || @@ -581,6 +590,7 @@ void Sensors::PostUpdate(const UpdateInfo &_info, _ecm.HasComponentType(components::SegmentationCamera::typeId) || _ecm.HasComponentType(components::BoundingBoxCamera::typeId))) { + std::unique_lock lock(this->dataPtr->renderMutex); igndbg << "Initialization needed" << std::endl; this->dataPtr->doInit = true; this->dataPtr->renderCv.notify_one(); @@ -589,57 +599,68 @@ void Sensors::PostUpdate(const UpdateInfo &_info, if (this->dataPtr->running && this->dataPtr->initialized) { - this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); - - auto time = math::durationToSecNsec(_info.simTime); - auto t = math::secNsecToDuration(time.first, time.second); - - std::vector activeSensors; - - this->dataPtr->sensorMaskMutex.lock(); - for (auto id : this->dataPtr->sensorIds) { - sensors::Sensor *s = this->dataPtr->sensorManager.Sensor(id); - auto rs = dynamic_cast(s); - - auto it = this->dataPtr->sensorMask.find(id); - if (it != this->dataPtr->sensorMask.end()) - { - if (it->second <= t) - { - this->dataPtr->sensorMask.erase(it); - } - else - { - continue; - } - } + std::unique_lock lock(this->dataPtr->renderMutex); + this->dataPtr->renderUtil.UpdateFromECM(_info, _ecm); + this->dataPtr->updateTime = _info.simTime; + } - if (rs && rs->NextDataUpdateTime() <= t) - { - activeSensors.push_back(rs); - } + // check connections to render events + // we will need to perform render updates if there are event subscribers + // \todo(anyone) This currently forces scene tree updates at the sim update + // rate which can be too frequent and causes a performance hit. + // We should look into throttling render updates + bool hasRenderConnections = + (this->dataPtr->eventManager->ConnectionCount() > 0u || + this->dataPtr->eventManager->ConnectionCount() > 0u); + + // if nextUpdateTime is max, it probably means there are previously + // no active sensors or sensors with connections. + // In this case, check if sensors have connections now. If so, we need to + // set the nextUpdateTime + if (this->dataPtr->nextUpdateTime == + std::chrono::steady_clock::duration::max() && + this->dataPtr->SensorsHaveConnections()) + { + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); } - this->dataPtr->sensorMaskMutex.unlock(); - if (!activeSensors.empty() || - this->dataPtr->renderUtil.PendingSensors() > 0) + // notify the render thread if updates are available + if (hasRenderConnections || + this->dataPtr->nextUpdateTime <= _info.simTime || + this->dataPtr->renderUtil.PendingSensors() > 0 || + this->dataPtr->forceUpdate) { if (this->dataPtr->disableOnDrainedBattery) this->dataPtr->UpdateBatteryState(_ecm); - std::unique_lock lock(this->dataPtr->renderMutex); - this->dataPtr->renderCv.wait(lock, [this] { - return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); - + { + std::unique_lock cvLock(this->dataPtr->renderMutex); + this->dataPtr->renderCv.wait(cvLock, [this] { + return !this->dataPtr->running || !this->dataPtr->updateAvailable; }); + } if (!this->dataPtr->running) { return; } - this->dataPtr->activeSensors = std::move(activeSensors); - this->dataPtr->updateTime = t; - this->dataPtr->updateAvailable = true; + { + std::unique_lock lockSensors(this->dataPtr->sensorsMutex); + this->dataPtr->activeSensors = + std::move(this->dataPtr->sensorsToUpdate); + + this->dataPtr->nextUpdateTime = this->dataPtr->NextUpdateTime( + this->dataPtr->sensorsToUpdate, _info.simTime); + + // Force scene tree update if there are sensors to be created or + // subscribes to the render events. This does not necessary force + // sensors to update. Only active sensors will be updated + this->dataPtr->forceUpdate = + (this->dataPtr->renderUtil.PendingSensors() > 0) || + hasRenderConnections; + this->dataPtr->updateAvailable = true; + } this->dataPtr->renderCv.notify_one(); } } @@ -889,6 +910,86 @@ bool SensorsPrivate::HasConnections(sensors::RenderingSensor *_sensor) const return true; } +////////////////////////////////////////////////// +std::chrono::steady_clock::duration SensorsPrivate::NextUpdateTime( + std::set &_sensorsToUpdate, + const std::chrono::steady_clock::duration &_currentTime) +{ + _sensorsToUpdate.clear(); + std::chrono::steady_clock::duration minNextUpdateTime = + std::chrono::steady_clock::duration::max(); + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (!this->HasConnections(rs)) + { + continue; + } + + std::chrono::steady_clock::duration time; + // if sensor's next update tims is less or equal to current sim time then + // it's in the process of being updated by the render loop + // Set their next update time to be current time + update period + if (rs->NextDataUpdateTime() <= _currentTime) + { + time = rs->NextDataUpdateTime() + + std::chrono::duration_cast( + std::chrono::duration(1.0 / rs->UpdateRate())); + } + else + { + time = rs->NextDataUpdateTime(); + } + + if (time <= minNextUpdateTime) + { + _sensorsToUpdate.clear(); + minNextUpdateTime = time; + } + _sensorsToUpdate.insert(id); + } + return minNextUpdateTime; +} + +////////////////////////////////////////////////// +bool SensorsPrivate::SensorsHaveConnections() +{ + for (auto id : this->sensorIds) + { + sensors::Sensor *s = this->sensorManager.Sensor(id); + if (nullptr == s) + { + continue; + } + + auto rs = dynamic_cast(s); + + if (nullptr == rs) + { + continue; + } + + if (this->HasConnections(rs)) + { + return true; + } + } + return false; +} + IGNITION_ADD_PLUGIN(Sensors, System, Sensors::ISystemConfigure, Sensors::ISystemUpdate, diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 2606428c13..af957b4769 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -99,6 +99,7 @@ set(tests_needing_display rgbd_camera.cc sensors_system.cc sensors_system_battery.cc + sensors_system_update_rate.cc shader_param_system.cc thermal_sensor_system.cc thermal_system.cc diff --git a/test/integration/sensors_system_update_rate.cc b/test/integration/sensors_system_update_rate.cc new file mode 100644 index 0000000000..283717bfeb --- /dev/null +++ b/test/integration/sensors_system_update_rate.cc @@ -0,0 +1,311 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include +#include + +#include +#include + +#include "gz/sim/components/Camera.hh" +#include "gz/sim/components/DepthCamera.hh" +#include "gz/sim/components/GpuLidar.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Physics.hh" +#include "gz/sim/components/RgbdCamera.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/SegmentationCamera.hh" +#include "gz/sim/components/ThermalCamera.hh" +#include "gz/sim/components/World.hh" + +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/EventManager.hh" +#include "gz/sim/Server.hh" +#include "gz/sim/SystemLoader.hh" +#include "gz/sim/Types.hh" + +#include "plugins/MockSystem.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace std::chrono_literals; +namespace components = gz::sim::components; + +////////////////////////////////////////////////// +class SensorsFixture : public InternalFixture> +{ + protected: void SetUp() override + { + InternalFixture::SetUp(); + + sdf::Plugin sdfPlugin; + sdfPlugin.SetFilename("libMockSystem.so"); + sdfPlugin.SetName("gz::sim::MockSystem"); + auto plugin = sm.LoadPlugin(sdfPlugin); + EXPECT_TRUE(plugin.has_value()); + this->systemPtr = plugin.value(); + this->mockSystem = static_cast( + systemPtr->QueryInterface()); + } + + public: gz::sim::SystemPluginPtr systemPtr; + public: sim::MockSystem *mockSystem; + + private: sim::SystemLoader sm; +}; + +///////////////////////////////////////////////// +TEST_F(SensorsFixture, IGN_UTILS_TEST_DISABLED_ON_MAC(UpdateRate)) +{ + gz::sim::ServerConfig serverConfig; + + const std::string sdfFile = + common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "sensor.sdf"); + + serverConfig.SetSdfFile(sdfFile); + + sim::Server server(serverConfig); + + // A pointer to the ecm. This will be valid once we run the mock system + sim::EntityComponentManager *ecm = nullptr; + this->mockSystem->configureCallback = + [&](const sim::Entity &, + const std::shared_ptr &, + sim::EntityComponentManager &_ecm, + sim::EventManager &) + { + ecm = &_ecm; + }; + + server.AddSystem(this->systemPtr); + transport::Node node; + std::string prefix{"/world/camera_sensor/model/default_topics/"}; + + std::vector imageTimestamps; + unsigned int imageCount = 0u; + auto cameraCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + imageTimestamps.push_back(t); + ++imageCount; + }); + node.Subscribe(prefix + "link/camera_link/sensor/camera/image", cameraCb); + + std::vector lidarTimestamps; + unsigned int lidarCount = 0u; + auto lidarCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + lidarTimestamps.push_back(t); + ++lidarCount; + }); + node.Subscribe(prefix + "link/gpu_lidar_link/sensor/gpu_lidar/scan", lidarCb); + + std::vector depthTimestamps; + unsigned int depthCount = 0u; + auto depthCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + depthTimestamps.push_back(t); + ++depthCount; + }); + node.Subscribe( + prefix + "link/depth_camera_link/sensor/depth_camera/depth_image", + depthCb); + + std::vector rgbdTimestamps; + unsigned int rgbdCount = 0u; + auto rgbdCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + rgbdTimestamps.push_back(t); + ++rgbdCount; + }); + node.Subscribe( + prefix + "link/rgbd_camera_link/sensor/rgbd_camera/image", + rgbdCb); + + std::vector thermalTimestamps; + unsigned int thermalCount = 0u; + auto thermalCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + thermalTimestamps.push_back(t); + ++thermalCount; + }); + node.Subscribe( + prefix + "link/thermal_camera_link/sensor/thermal_camera/image", + thermalCb); + + std::vector segmentationTimestamps; + unsigned int segmentationCount = 0u; + auto segmentationCb = std::function( + [&](const auto &_msg) + { + double t = _msg.header().stamp().sec() + + _msg.header().stamp().nsec() * 1e-9; + segmentationTimestamps.push_back(t); + ++segmentationCount; + }); + node.Subscribe( + prefix + "link/segmentation_camera_link/sensor/segmentation_camera/" + + "segmentation/colored_map", + segmentationCb); + + unsigned int iterations = 2000u; + server.Run(true, iterations, false); + + EXPECT_NE(nullptr, ecm); + + // get the world step size + auto worldEntity = ecm->EntityByComponents(components::World()); + EXPECT_NE(sim::kNullEntity, worldEntity); + auto physicsSdf = ecm->Component(worldEntity)->Data(); + double stepSize = physicsSdf.MaxStepSize(); + EXPECT_LT(0, stepSize); + + // get the sensors update rates + auto camLinkEntity = ecm->EntityByComponents(components::Name("camera_link")); + EXPECT_NE(sim::kNullEntity, camLinkEntity); + auto camEntity = ecm->EntityByComponents(components::Name("camera"), + components::ParentEntity(camLinkEntity)); + EXPECT_NE(sim::kNullEntity, camEntity); + auto sensorSdf = ecm->Component(camEntity)->Data(); + unsigned int camRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, camRate); + + auto lidarEntity = ecm->EntityByComponents(components::Name("gpu_lidar")); + EXPECT_NE(sim::kNullEntity, lidarEntity); + sensorSdf = ecm->Component(lidarEntity)->Data(); + unsigned int lidarRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, lidarRate); + + auto depthEntity = ecm->EntityByComponents(components::Name("depth_camera")); + EXPECT_NE(sim::kNullEntity, depthEntity); + sensorSdf = ecm->Component(depthEntity)->Data(); + unsigned int depthRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, depthRate); + + auto rgbdEntity = ecm->EntityByComponents(components::Name("rgbd_camera")); + EXPECT_NE(sim::kNullEntity, rgbdEntity); + sensorSdf = ecm->Component(rgbdEntity)->Data(); + unsigned int rgbdRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, rgbdRate); + + auto thermalEntity = ecm->EntityByComponents( + components::Name("thermal_camera")); + EXPECT_NE(sim::kNullEntity, thermalEntity); + sensorSdf = ecm->Component(thermalEntity)->Data(); + unsigned int thermalRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, thermalRate); + + auto segmentationEntity = ecm->EntityByComponents( + components::Name("segmentation_camera")); + EXPECT_NE(sim::kNullEntity, segmentationEntity); + sensorSdf = ecm->Component( + segmentationEntity)->Data(); + unsigned int segmentationRate = sensorSdf.UpdateRate(); + EXPECT_LT(0u, segmentationRate); + + // compute and verify expected msg count based on update rate and sim time + double timeRan = iterations * stepSize; + + unsigned int expectedCamMsgCount = timeRan / (1.0 / camRate) + 1; + unsigned int expectedDepthMsgCount = timeRan / (1.0 / depthRate) + 1; + unsigned int expectedLidarMsgCount = timeRan / (1.0 / lidarRate) + 1; + unsigned int expectedRgbdMsgCount = timeRan / (1.0 / rgbdRate) + 1; + unsigned int expectedThermalMsgCount = timeRan / (1.0 / thermalRate) + 1; + unsigned int expectedSegmentationMsgCount = + timeRan / (1.0 / segmentationRate) + 1; + + unsigned int sleep = 0; + unsigned int maxSleep = 100; + while (sleep++ < maxSleep && + (imageCount < expectedCamMsgCount || + lidarCount < expectedLidarMsgCount || + depthCount < expectedDepthMsgCount || + rgbdCount < expectedRgbdMsgCount || + thermalCount < expectedThermalMsgCount || + segmentationCount < expectedSegmentationMsgCount)) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_EQ(expectedCamMsgCount, imageCount); + EXPECT_EQ(expectedDepthMsgCount, depthCount); + EXPECT_EQ(expectedLidarMsgCount, lidarCount); + EXPECT_EQ(expectedRgbdMsgCount, rgbdCount); + EXPECT_EQ(expectedThermalMsgCount, thermalCount); + EXPECT_EQ(expectedSegmentationMsgCount, segmentationCount); + + // verify timestamps + // The first timestamp may not be 0 because the rendering + // may take some time to start and it does not block the main thread + // so start with index = 1 + // \todo(anyone) Make the sensors system thread block so we always + // generate data at t = 0? + EXPECT_FALSE(imageTimestamps.empty()); + EXPECT_FALSE(lidarTimestamps.empty()); + EXPECT_FALSE(depthTimestamps.empty()); + EXPECT_FALSE(rgbdTimestamps.empty()); + EXPECT_FALSE(thermalTimestamps.empty()); + EXPECT_FALSE(segmentationTimestamps.empty()); + for (unsigned int i = 1; i < imageTimestamps.size()-1; ++i) + { + double dt = imageTimestamps[i+1] - imageTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / camRate, dt); + } + for (unsigned int i = 1; i < lidarTimestamps.size()-1; ++i) + { + double dt = lidarTimestamps[i+1] - lidarTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / lidarRate, dt); + } + for (unsigned int i = 1; i < depthTimestamps.size()-1; ++i) + { + double dt = depthTimestamps[i+1] - depthTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / depthRate, dt); + } + for (unsigned int i = 1; i < rgbdTimestamps.size()-1; ++i) + { + double dt = rgbdTimestamps[i+1] - rgbdTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / rgbdRate, dt); + } + for (unsigned int i = 1; i < thermalTimestamps.size()-1; ++i) + { + double dt = thermalTimestamps[i+1] - thermalTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / thermalRate, dt); + } + for (unsigned int i = 1; i < segmentationTimestamps.size()-1; ++i) + { + double dt = segmentationTimestamps[i+1] - segmentationTimestamps[i]; + EXPECT_FLOAT_EQ(1.0 / segmentationRate, dt); + } +} diff --git a/test/worlds/sensor.sdf b/test/worlds/sensor.sdf index 428c86d2fc..27338807f1 100644 --- a/test/worlds/sensor.sdf +++ b/test/worlds/sensor.sdf @@ -2,7 +2,8 @@ - 0 + 0.001 + 1.0 true + 50 1.047 @@ -103,6 +105,7 @@ " + 40 @@ -129,6 +132,7 @@ + 25 1.05 @@ -152,6 +156,7 @@ + 10 1.05 @@ -168,6 +173,7 @@ + 40 1.15 @@ -184,6 +190,7 @@ + 25 1.0 From 458782745a467698d40511066f7521b616b179ba Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Mon, 31 Jul 2023 10:23:31 -0500 Subject: [PATCH 14/18] Use sdf::Element::FindElement instead of GetElement in ApplyLinkWrench (#2052) This fixes errors such as ``` Error [Utils.cc:174] Missing element description for [persistent] ``` Signed-off-by: Addisu Z. Taddese --- src/systems/apply_link_wrench/ApplyLinkWrench.cc | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/src/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 295dd8e644..4a11ce8f36 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -148,8 +148,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, this->dataPtr->verbose = _sdf->Get("verbose", true).first; // Initial wrenches - auto ptr = const_cast(_sdf.get()); - for (auto elem = ptr->GetElement("persistent"); + for (auto elem = _sdf->FindElement("persistent"); elem != nullptr; elem = elem->GetNextElement("persistent")) { @@ -163,7 +162,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, msg.mutable_entity()->set_name(elem->Get("entity_name")); - auto typeStr = elem->GetElement("entity_type")->Get(); + auto typeStr = elem->FindElement("entity_type")->Get(); if (typeStr == "link") { msg.mutable_entity()->set_type(msgs::Entity::LINK); @@ -182,12 +181,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); } From 4d31281b964f08f627a81687967ea551d29aa8b1 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Wed, 2 Aug 2023 20:35:28 +0800 Subject: [PATCH 15/18] Fix a minor issue in the documentation of the server API (#2067) Signed-off-by: Arjo Chakravarty --- include/gz/sim/Server.hh | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index 0f2806fe37..2dfba75b4d 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -80,13 +80,15 @@ namespace ignition /// /// 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) : From 7713f2ba90b579cd9c710c84063e8e61c95fa587 Mon Sep 17 00:00:00 2001 From: Arjo Chakravarty Date: Fri, 4 Aug 2023 00:38:37 +0800 Subject: [PATCH 16/18] Fix Joint Position Controller Behaviour Described in #1997 (#2001) * Fix Joint Position Controller Behaviour Described in #1997 There are several issues at play. First the target velcity calculation was wrong as described in #1997. Secondly, even if we corrected that there was still incorrect behaviour. This is due to the fact that we use the PID's CmdMax to determine what the maximum velocity for the joint is. However, in the event a user does not set a `` this defaults to zero and the joint does not move. Finally this PR updates the tests. Previously our tests were only testing the case where the position command was well above the position's maximum velocity, hence it would slide into position. This PR introduces a test where we snap the position of the joint into place instead. Signed-off-by: Arjo Chakravarty * Address PR feedback with regards to style. Signed-off-by: Arjo Chakravarty * Just one more thing Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * More minor style fixes Signed-off-by: Arjo Chakravarty * More minor style fixes Signed-off-by: Arjo Chakravarty * Tracks periodic changes in scene broadcaster (#2010) * Tracks periodic changes in scene broadcaster This commit proposes a change to the scene broadcaster which enables tracking of all components with periodic changes. This way if a component has a periodic change the scene broadcaster does not miss it. For more info see this discussion https://github.com/gazebosim/gz-sim/pull/2001#issuecomment-1581573728 where @azeey proposes this solution. This commit is WIP and I still need to handle entity/component removal and add tests. Signed-off-by: Arjo Chakravarty * Rework changes * Removes clone made of BaseComponent. Signed-off-by: Arjo Chakravarty * More reworks of added APIs to ECM. Signed-off-by: Arjo Chakravarty * Fix tests Signed-off-by: Arjo Chakravarty * Add ECM related tests. Signed-off-by: Arjo Chakravarty * Address spelling issue Co-authored-by: Michael Carroll Signed-off-by: Arjo Chakravarty * Get rid of TODO Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Co-authored-by: Michael Carroll * Migrate to new header. Signed-off-by: Arjo Chakravarty * Update test paths Signed-off-by: Arjo Chakravarty * Update include/gz/sim/EntityComponentManager.hh Co-authored-by: Addisu Z. Taddese Signed-off-by: Arjo Chakravarty * Rename methods Signed-off-by: Arjo Chakravarty * style Signed-off-by: Arjo Chakravarty * Flipped data structure around Signed-off-by: Arjo Chakravarty * Fix GCC warning Signed-off-by: Arjo Chakravarty --------- Signed-off-by: Arjo Chakravarty Signed-off-by: Arjo Chakravarty Co-authored-by: Michael Carroll Co-authored-by: Addisu Z. Taddese --- include/gz/sim/EntityComponentManager.hh | 24 ++++ src/EntityComponentManager.cc | 78 ++++++++++++ src/EntityComponentManager_TEST.cc | 66 ++++++++++ .../JointPositionController.cc | 8 +- .../scene_broadcaster/SceneBroadcaster.cc | 27 ++-- .../joint_position_controller_system.cc | 118 ++++++++++++++++-- .../joint_position_controller_velocity.sdf | 79 ++++++++++++ 7 files changed, 379 insertions(+), 21 deletions(-) diff --git a/include/gz/sim/EntityComponentManager.hh b/include/gz/sim/EntityComponentManager.hh index fa33557129..97b0ba1a74 100644 --- a/include/gz/sim/EntityComponentManager.hh +++ b/include/gz/sim/EntityComponentManager.hh @@ -601,6 +601,17 @@ namespace ignition 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. @@ -628,6 +639,19 @@ namespace ignition 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/src/EntityComponentManager.cc b/src/EntityComponentManager.cc index d768971aa4..c03c4b911b 100644 --- a/src/EntityComponentManager.cc +++ b/src/EntityComponentManager.cc @@ -963,6 +963,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 { @@ -1678,6 +1714,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 28ff2083b6..56cc5fe363 100644 --- a/src/EntityComponentManager_TEST.cc +++ b/src/EntityComponentManager_TEST.cc @@ -2204,6 +2204,72 @@ TEST_P(EntityComponentManagerFixture, Descendants) } } +////////////////////////////////////////////////// +TEST_P(EntityComponentManagerFixture, + IGN_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, IGN_UTILS_TEST_DISABLED_ON_WIN32(SetChanged)) diff --git a/src/systems/joint_position_controller/JointPositionController.cc b/src/systems/joint_position_controller/JointPositionController.cc index 96911d7776..9ec4d0433a 100644 --- a/src/systems/joint_position_controller/JointPositionController.cc +++ b/src/systems/joint_position_controller/JointPositionController.cc @@ -57,6 +57,9 @@ class ignition::gazebo::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}; @@ -149,6 +152,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")) { @@ -306,14 +310,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; } // Set velocity and return diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index 80256b936c..0d986c2567 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 @@ -257,8 +258,15 @@ class ignition::gazebo::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()) @@ -341,6 +349,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: @@ -383,15 +394,7 @@ void SceneBroadcaster::PostUpdate(const UpdateInfo &_info, else if (!_info.paused) { IGN_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 @@ -416,6 +419,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/test/integration/joint_position_controller_system.cc b/test/integration/joint_position_controller_system.cc index f91788334a..fbc850e14a 100644 --- a/test/integration/joint_position_controller_system.cc +++ b/test/integration/joint_position_controller_system.cc @@ -56,8 +56,8 @@ TEST_F(JointPositionControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_position_controller.sdf"; + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "/test/worlds/joint_position_controller.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -133,8 +133,8 @@ TEST_F(JointPositionControllerTestFixture, // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_position_controller_velocity.sdf"; + const auto sdfFile = common::joinPaths(PROJECT_SOURCE_PATH, + "test", "worlds", "joint_position_controller_velocity.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); @@ -170,8 +170,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; }); }); @@ -184,7 +184,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); @@ -200,10 +200,108 @@ 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 respects the maximum command +TEST_F(JointPositionControllerTestFixture, + IGN_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/worlds/joint_position_controller_velocity.sdf b/test/worlds/joint_position_controller_velocity.sdf index b7f5fdc774..cf3a9c5080 100644 --- a/test/worlds/joint_position_controller_velocity.sdf +++ b/test/worlds/joint_position_controller_velocity.sdf @@ -111,6 +111,85 @@ name="ignition::gazebo::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 From b79c9bc2ddb40016f339d7fac0a19b322aa998a1 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 15 Aug 2023 16:13:38 -0500 Subject: [PATCH 17/18] Merge ign-gazebo3 to ign-gazebo6 Signed-off-by: Addisu Z. Taddese --- include/gz/sim/Server.hh | 8 +++++--- src/TestFixture.cc | 7 +++++++ src/systems/apply_link_wrench/ApplyLinkWrench.cc | 9 ++++----- 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/include/gz/sim/Server.hh b/include/gz/sim/Server.hh index 2c8583d962..236e3bb634 100644 --- a/include/gz/sim/Server.hh +++ b/include/gz/sim/Server.hh @@ -80,13 +80,15 @@ namespace ignition /// /// 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/TestFixture.cc b/src/TestFixture.cc index 7e05455ed2..0b6e90a258 100644 --- a/src/TestFixture.cc +++ b/src/TestFixture.cc @@ -210,6 +210,13 @@ TestFixture &TestFixture::OnPostUpdate(std::function TestFixture::Server() const { + if (!this->dataPtr->finalized) + { + ignwarn << "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/systems/apply_link_wrench/ApplyLinkWrench.cc b/src/systems/apply_link_wrench/ApplyLinkWrench.cc index 295dd8e644..4a11ce8f36 100644 --- a/src/systems/apply_link_wrench/ApplyLinkWrench.cc +++ b/src/systems/apply_link_wrench/ApplyLinkWrench.cc @@ -148,8 +148,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, this->dataPtr->verbose = _sdf->Get("verbose", true).first; // Initial wrenches - auto ptr = const_cast(_sdf.get()); - for (auto elem = ptr->GetElement("persistent"); + for (auto elem = _sdf->FindElement("persistent"); elem != nullptr; elem = elem->GetNextElement("persistent")) { @@ -163,7 +162,7 @@ void ApplyLinkWrench::Configure(const Entity &_entity, msg.mutable_entity()->set_name(elem->Get("entity_name")); - auto typeStr = elem->GetElement("entity_type")->Get(); + auto typeStr = elem->FindElement("entity_type")->Get(); if (typeStr == "link") { msg.mutable_entity()->set_type(msgs::Entity::LINK); @@ -182,12 +181,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); } From 326cef2b4a177bf6ea957fe9e2841c4719d598db Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Wed, 16 Aug 2023 13:28:03 -0500 Subject: [PATCH 18/18] Prepare for 6.15.0 (#2080) Signed-off-by: Addisu Z. Taddese --- CMakeLists.txt | 2 +- Changelog.md | 167 +++++++++++++++++++++++++++++++++++++++++++++++++ 2 files changed, 168 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6453a20d7e..b6a0558c5b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,7 +3,7 @@ cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) #============================================================================ # Initialize the project #============================================================================ -project(ignition-gazebo6 VERSION 6.14.0) +project(ignition-gazebo6 VERSION 6.15.0) set (GZ_DISTRIBUTION "Fortress") #============================================================================ diff --git a/Changelog.md b/Changelog.md index b3b396f389..07b126f624 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,172 @@ ## Ignition Gazebo 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