diff --git a/gazebo/gui/ApplyWrenchDialog.cc b/gazebo/gui/ApplyWrenchDialog.cc index 27de07203d..04119ea8bc 100644 --- a/gazebo/gui/ApplyWrenchDialog.cc +++ b/gazebo/gui/ApplyWrenchDialog.cc @@ -486,6 +486,13 @@ bool ApplyWrenchDialog::SetModel(const std::string &_modelName) for (unsigned int i = 0; i < vis->GetChildCount(); ++i) { rendering::VisualPtr childVis = vis->GetChild(i); + + // Skip all children that aren't links + if (childVis->GetType() != rendering::Visual::VT_LINK) + { + continue; + } + std::string linkName = childVis->Name(); // Issue #1553: This is failing to get real links sometimes: @@ -493,7 +500,15 @@ bool ApplyWrenchDialog::SetModel(const std::string &_modelName) // if (!((flags != GZ_VISIBILITY_ALL) && (flags & GZ_VISIBILITY_GUI))) if (linkName.find("_GL_MANIP_") == std::string::npos) { - std::string unscopedLinkName = linkName.substr(linkName.find("::") + 2); + size_t modelNamePos = linkName.find(_modelName); + // Skip if the model name isn't in the scoped link name + if (modelNamePos == std::string::npos) + { + continue; + } + std::string unscopedLinkName = linkName.substr(modelNamePos + + + _modelName.size() + 2); + this->dataPtr->linksComboBox->addItem( QString::fromStdString(unscopedLinkName)); @@ -538,8 +553,18 @@ bool ApplyWrenchDialog::SetLink(const std::string &_linkName) if (!gui::get_active_camera() || !gui::get_active_camera()->GetScene()) return false; - // Select on combo box - std::string unscopedLinkName = _linkName.substr(_linkName.find("::") + 2); + size_t modelNamePos = _linkName.find(this->dataPtr->modelName); + std::string unscopedLinkName; + + // Leave unscopedLinkName empty if model name cannot be found + // in the scoped link name + if (modelNamePos != std::string::npos) + { + // Select on combo box + unscopedLinkName = _linkName.substr(modelNamePos + + this->dataPtr->modelName.size() + 2); + } + int index = -1; for (int i = 0; i < this->dataPtr->linksComboBox->count(); ++i) { diff --git a/gazebo/gui/ApplyWrenchDialog_TEST.cc b/gazebo/gui/ApplyWrenchDialog_TEST.cc index 7e64d0a76b..00f7d35ada 100644 --- a/gazebo/gui/ApplyWrenchDialog_TEST.cc +++ b/gazebo/gui/ApplyWrenchDialog_TEST.cc @@ -32,7 +32,7 @@ void ApplyWrenchDialog_TEST::ApplyForceTorqueFromDialog() this->shareMaxPercentChange = 2.0; // World with one model which has 2 links, no ground plane and gravity is off - this->Load("worlds/multilink_shape.world", false, false, false); + this->Load("worlds/nested_multilink_shape.world", false, false, false); // Create the main window. gazebo::gui::MainWindow *mainWindow = new gazebo::gui::MainWindow(); @@ -49,200 +49,221 @@ void ApplyWrenchDialog_TEST::ApplyForceTorqueFromDialog() gazebo::rendering::ScenePtr scene = cam->GetScene(); QVERIFY(scene != nullptr); - // Get the model - gazebo::rendering::VisualPtr modelVis = scene->GetVisual("multilink"); - QVERIFY(modelVis != nullptr); - - // Get the box link - gazebo::rendering::VisualPtr boxLinkVis = - scene->GetVisual("multilink::box_link"); - QVERIFY(boxLinkVis != nullptr); - auto boxLinkPose = boxLinkVis->WorldPose(); - QVERIFY(boxLinkPose == boxLinkVis->WorldPose()); - - // Get the sphere link - gazebo::rendering::VisualPtr sphereLinkVis = - scene->GetVisual("multilink::sphere_link"); - QVERIFY(sphereLinkVis != nullptr); - auto sphereLinkPose = sphereLinkVis->WorldPose(); - QVERIFY(sphereLinkPose == sphereLinkVis->WorldPose()); - - // Check that an inexistent model doesn't break anything - gazebo::gui::ApplyWrenchDialog *applyWrenchDialogFakeModel = - new gazebo::gui::ApplyWrenchDialog(); - applyWrenchDialogFakeModel->Init("fake_model", "fake_link"); - - // Check that an inexistent link doesn't break anything - gazebo::gui::ApplyWrenchDialog *applyWrenchDialogFakeLink = - new gazebo::gui::ApplyWrenchDialog(); - applyWrenchDialogFakeLink->Init("multilink", "fake_link"); - - // Initialize dialog with the box link - gazebo::gui::ApplyWrenchDialog *applyWrenchDialog = - new gazebo::gui::ApplyWrenchDialog(); - applyWrenchDialog->Init("multilink", "multilink::box_link"); + // Test parameters for regular and nested models + const unsigned int testCount = 2; + std::string modelNameList[testCount] = + {"multilink", "nested_outer::nested_inner"}; + std::string boxLinkList[testCount] = {"box_link", "nested_box_link"}; + std::string sphereLinkList[testCount] = {"sphere_link", "nested_sphere_link"}; - // Get combo box - QList comboBoxes = - applyWrenchDialog->findChildren(); - QVERIFY(comboBoxes.size() == 1u); - - // Check the combo box's items - QVERIFY(comboBoxes[0]->count() == 2u); - QVERIFY(comboBoxes[0]->itemText(0) == "box_link"); - QVERIFY(comboBoxes[0]->itemText(1) == "sphere_link"); - QVERIFY(comboBoxes[0]->currentIndex() == 0u); - - // Get radio buttons - QList radioButtons = - applyWrenchDialog->findChildren(); - QVERIFY(radioButtons.size() == 2u); - - // Get spins - QList spins = - applyWrenchDialog->findChildren(); - QVERIFY(spins.size() == 11u); - - // Get buttons - QList buttons = - applyWrenchDialog->findChildren(); - QVERIFY(buttons.size() == 6u); - - QPushButton *applyForceButton = nullptr; - QPushButton *applyTorqueButton = nullptr; - QPushButton *applyAllButton = nullptr; - QPushButton *clearForceButton = nullptr; - QPushButton *clearTorqueButton = nullptr; - QPushButton *cancelButton = nullptr; - for (auto it : buttons) + for (unsigned int i = 0; i < testCount; ++i) { - QVERIFY(it); - if (it->text().toLower().toStdString() == "apply force") - applyForceButton = it; - else if (it->text().toLower().toStdString() == "apply torque") - applyTorqueButton = it; - else if (it->text().toLower().toStdString() == "apply all") - applyAllButton = it; - else if (it->text().toLower().toStdString() == "clear" && !clearForceButton) - clearForceButton = it; - else if (it->text().toLower().toStdString() == "clear") - clearTorqueButton = it; - else if (it->text().toLower().toStdString() == "cancel") - cancelButton = it; - } - QVERIFY(applyForceButton); - QVERIFY(applyTorqueButton); - QVERIFY(applyAllButton); - QVERIFY(clearForceButton); - QVERIFY(clearTorqueButton); - QVERIFY(cancelButton); - - // Set and apply force on X axis, magnitude 1000 - spins[0]->setValue(1.0); - spins[3]->setValue(1000.0); - applyForceButton->click(); + // QString defines operator== for c-strings, but not std::string + const char *currentModel = modelNameList[i].c_str(); + const char *currentUnscopedBoxLink = boxLinkList[i].c_str(); + const char *currentUnscopedSphereLink = sphereLinkList[i].c_str(); + std::string _scopedBoxLink = modelNameList[i] + "::" + boxLinkList[i]; + std::string _scopedSphereLink = modelNameList[i] + "::" + sphereLinkList[i]; + const char *currentScopedBoxLink = _scopedBoxLink.c_str(); + const char *currentScopedSphereLink = _scopedSphereLink.c_str(); + + // Get the model + gazebo::rendering::VisualPtr modelVis = scene->GetVisual(modelNameList[i]); + QVERIFY(modelVis != nullptr); + + // Get the box link + gazebo::rendering::VisualPtr boxLinkVis = + scene->GetVisual(currentScopedBoxLink); + QVERIFY(boxLinkVis != nullptr); + auto boxLinkPose = boxLinkVis->WorldPose(); + QVERIFY(boxLinkPose == boxLinkVis->WorldPose()); + + // Get the sphere link + gazebo::rendering::VisualPtr sphereLinkVis = + scene->GetVisual(currentScopedSphereLink); + QVERIFY(sphereLinkVis != nullptr); + auto sphereLinkPose = sphereLinkVis->WorldPose(); + QVERIFY(sphereLinkPose == sphereLinkVis->WorldPose()); + + // Check that an inexistent model doesn't break anything + gazebo::gui::ApplyWrenchDialog *applyWrenchDialogFakeModel = + new gazebo::gui::ApplyWrenchDialog(); + applyWrenchDialogFakeModel->Init("fake_model", "fake_link"); + + // Check that an inexistent link doesn't break anything + gazebo::gui::ApplyWrenchDialog *applyWrenchDialogFakeLink = + new gazebo::gui::ApplyWrenchDialog(); + applyWrenchDialogFakeLink->Init(currentModel, "fake_link"); + + // Initialize dialog with the box link + gazebo::gui::ApplyWrenchDialog *applyWrenchDialog = + new gazebo::gui::ApplyWrenchDialog(); + applyWrenchDialog->Init(currentModel, currentScopedBoxLink); + + // Get combo box + QList comboBoxes = + applyWrenchDialog->findChildren(); + QVERIFY(comboBoxes.size() == 1u); + + // Check the combo box's items + QVERIFY(comboBoxes[0]->count() == 2u); + QVERIFY(comboBoxes[0]->itemText(0) == currentUnscopedBoxLink); + QVERIFY(comboBoxes[0]->itemText(1) == currentUnscopedSphereLink); + QVERIFY(comboBoxes[0]->currentIndex() == 0u); + + // Get radio buttons + QList radioButtons = + applyWrenchDialog->findChildren(); + QVERIFY(radioButtons.size() == 2u); + + // Get spins + QList spins = + applyWrenchDialog->findChildren(); + QVERIFY(spins.size() == 11u); + + // Get buttons + QList buttons = + applyWrenchDialog->findChildren(); + QVERIFY(buttons.size() == 6u); + + QPushButton *applyForceButton = nullptr; + QPushButton *applyTorqueButton = nullptr; + QPushButton *applyAllButton = nullptr; + QPushButton *clearForceButton = nullptr; + QPushButton *clearTorqueButton = nullptr; + QPushButton *cancelButton = nullptr; + for (auto it : buttons) + { + QVERIFY(it); + if (it->text().toLower().toStdString() == "apply force") + applyForceButton = it; + else if (it->text().toLower().toStdString() == "apply torque") + applyTorqueButton = it; + else if (it->text().toLower().toStdString() == "apply all") + applyAllButton = it; + else if (it->text().toLower().toStdString() == "clear" && + !clearForceButton) + clearForceButton = it; + else if (it->text().toLower().toStdString() == "clear") + clearTorqueButton = it; + else if (it->text().toLower().toStdString() == "cancel") + cancelButton = it; + } + QVERIFY(applyForceButton); + QVERIFY(applyTorqueButton); + QVERIFY(applyAllButton); + QVERIFY(clearForceButton); + QVERIFY(clearTorqueButton); + QVERIFY(cancelButton); - this->ProcessEventsAndDraw(mainWindow); + // Set and apply force on X axis, magnitude 1000 + spins[0]->setValue(1.0); + spins[3]->setValue(1000.0); + applyForceButton->click(); - // Check that force spin was updated according to magnitude - QCOMPARE(spins[0]->value(), 1000.0); + this->ProcessEventsAndDraw(mainWindow); - // Check that link moved on X axis - QVERIFY(boxLinkPose.Pos().X() < boxLinkVis->WorldPose().Pos().X()); - QVERIFY(boxLinkPose.Pos().Y() - - boxLinkVis->WorldPose().Pos().Y() < 1e-6); - QVERIFY(boxLinkPose.Pos().Z() - - boxLinkVis->WorldPose().Pos().Z() < 1e-6); - QCOMPARE(boxLinkPose.Rot(), boxLinkVis->WorldPose().Rot()); + // Check that force spin was updated according to magnitude + QCOMPARE(spins[0]->value(), 1000.0); - // Save current pose - boxLinkPose = boxLinkVis->WorldPose(); + // Check that link moved on X axis + QVERIFY(boxLinkPose.Pos().X() < boxLinkVis->WorldPose().Pos().X()); + QVERIFY(boxLinkPose.Pos().Y() - + boxLinkVis->WorldPose().Pos().Y() < 1e-6); + QVERIFY(boxLinkPose.Pos().Z() - + boxLinkVis->WorldPose().Pos().Z() < 1e-6); + QCOMPARE(boxLinkPose.Rot(), boxLinkVis->WorldPose().Rot()); - // Set and apply torque about -Z axis, magnitude 1000 - spins[9]->setValue(-1.0); - spins[10]->setValue(1000.0); - applyTorqueButton->click(); + // Save current pose + boxLinkPose = boxLinkVis->WorldPose(); - this->ProcessEventsAndDraw(mainWindow); + // Set and apply torque about -Z axis, magnitude 1000 + spins[9]->setValue(-1.0); + spins[10]->setValue(1000.0); + applyTorqueButton->click(); - // Check that torque spin was updated according to magnitude - QCOMPARE(spins[9]->value(), -1000.0); + this->ProcessEventsAndDraw(mainWindow); - // Check that link rotated - QVERIFY(boxLinkPose.Pos().X() < boxLinkVis->WorldPose().Pos().X()); - QVERIFY(boxLinkPose.Pos().Y() - - boxLinkVis->WorldPose().Pos().Y() < 1e-6); - QVERIFY(boxLinkPose.Pos().Z() - - boxLinkVis->WorldPose().Pos().Z() < 1e-6); - QVERIFY(boxLinkPose.Rot() != boxLinkVis->WorldPose().Rot()); + // Check that torque spin was updated according to magnitude + QCOMPARE(spins[9]->value(), -1000.0); - // Save current pose - boxLinkPose = boxLinkVis->WorldPose(); + // Check that link rotated + QVERIFY(boxLinkPose.Pos().X() < boxLinkVis->WorldPose().Pos().X()); + QVERIFY(boxLinkPose.Pos().Y() - + boxLinkVis->WorldPose().Pos().Y() < 1e-6); + QVERIFY(boxLinkPose.Pos().Z() - + boxLinkVis->WorldPose().Pos().Z() < 1e-6); + QVERIFY(boxLinkPose.Rot() != boxLinkVis->WorldPose().Rot()); - // Apply force and torque - applyAllButton->click(); + // Save current pose + boxLinkPose = boxLinkVis->WorldPose(); - this->ProcessEventsAndDraw(mainWindow); + // Apply force and torque + applyAllButton->click(); - // Check that link translated and rotated - QVERIFY(boxLinkPose.Pos() != boxLinkVis->WorldPose().Pos()); - QVERIFY(boxLinkPose.Rot() != boxLinkVis->WorldPose().Rot()); + this->ProcessEventsAndDraw(mainWindow); - // Change link - comboBoxes[0]->setCurrentIndex(1); - QVERIFY(comboBoxes[0]->currentText() == "sphere_link"); + // Check that link translated and rotated + QVERIFY(boxLinkPose.Pos() != boxLinkVis->WorldPose().Pos()); + QVERIFY(boxLinkPose.Rot() != boxLinkVis->WorldPose().Rot()); - // Clear force - clearForceButton->click(); - QCOMPARE(spins[0]->value(), 0.0); - QCOMPARE(spins[1]->value(), 0.0); - QCOMPARE(spins[2]->value(), 0.0); - QCOMPARE(spins[3]->value(), 0.0); + // Change link + comboBoxes[0]->setCurrentIndex(1); + QVERIFY(comboBoxes[0]->currentText() == currentUnscopedSphereLink); - // Clear torque - clearTorqueButton->click(); - QCOMPARE(spins[7]->value(), 0.0); - QCOMPARE(spins[8]->value(), 0.0); - QCOMPARE(spins[9]->value(), 0.0); - QCOMPARE(spins[10]->value(), 0.0); + // Clear force + clearForceButton->click(); + QCOMPARE(spins[0]->value(), 0.0); + QCOMPARE(spins[1]->value(), 0.0); + QCOMPARE(spins[2]->value(), 0.0); + QCOMPARE(spins[3]->value(), 0.0); - // Apply zero force and torque - applyAllButton->click(); + // Clear torque + clearTorqueButton->click(); + QCOMPARE(spins[7]->value(), 0.0); + QCOMPARE(spins[8]->value(), 0.0); + QCOMPARE(spins[9]->value(), 0.0); + QCOMPARE(spins[10]->value(), 0.0); - this->ProcessEventsAndDraw(mainWindow); + // Apply zero force and torque + applyAllButton->click(); - // Check that link didn't move - QVERIFY(sphereLinkPose.Pos() == sphereLinkVis->WorldPose().Pos()); - QVERIFY(sphereLinkPose.Rot() == sphereLinkVis->WorldPose().Rot()); + this->ProcessEventsAndDraw(mainWindow); - // Set and apply force on Y axis with an offset on X - spins[1]->setValue(1000.0); - spins[4]->setValue(1.0); - applyForceButton->click(); + // Check that link didn't move + QVERIFY(sphereLinkPose.Pos() == sphereLinkVis->WorldPose().Pos()); + QVERIFY(sphereLinkPose.Rot() == sphereLinkVis->WorldPose().Rot()); - this->ProcessEventsAndDraw(mainWindow); + // Set and apply force on Y axis with an offset on X + spins[1]->setValue(1000.0); + spins[4]->setValue(1.0); + applyForceButton->click(); - // Check that link translated and rotated - QVERIFY(sphereLinkPose.Pos() != sphereLinkVis->WorldPose().Pos()); - QVERIFY(sphereLinkPose.Rot() != sphereLinkVis->WorldPose().Rot()); + this->ProcessEventsAndDraw(mainWindow); - // Select CoM as application point - radioButtons[0]->click(); + // Check that link translated and rotated + QVERIFY(sphereLinkPose.Pos() != sphereLinkVis->WorldPose().Pos()); + QVERIFY(sphereLinkPose.Rot() != sphereLinkVis->WorldPose().Rot()); - this->ProcessEventsAndDraw(mainWindow); + // Select CoM as application point + radioButtons[0]->click(); - // Check that force offset spins were updated - QCOMPARE(spins[4]->value(), 0.0); - QCOMPARE(spins[5]->value(), 0.0); - QCOMPARE(spins[6]->value(), 0.0); + this->ProcessEventsAndDraw(mainWindow); - // Close dialog - cancelButton->click(); + // Check that force offset spins were updated + QCOMPARE(spins[4]->value(), 0.0); + QCOMPARE(spins[5]->value(), 0.0); + QCOMPARE(spins[6]->value(), 0.0); - this->ProcessEventsAndDraw(mainWindow); + // Close dialog + cancelButton->click(); - delete applyWrenchDialog; + this->ProcessEventsAndDraw(mainWindow); + + delete applyWrenchDialog; + } + // Cleanup cam->Fini(); mainWindow->close(); delete mainWindow; diff --git a/gazebo/gui/ModelRightMenu.cc b/gazebo/gui/ModelRightMenu.cc index 11c4adf8ef..6e37dde5ad 100644 --- a/gazebo/gui/ModelRightMenu.cc +++ b/gazebo/gui/ModelRightMenu.cc @@ -169,15 +169,8 @@ ModelRightMenu::~ModelRightMenu() void ModelRightMenu::Run(const std::string &_entityName, const QPoint &_pt, EntityTypes _type) { - // Find out the entity type - if (_type == EntityTypes::MODEL) - { - this->entityName = _entityName.substr(0, _entityName.find("::")); - } - else if (_type == EntityTypes::LINK || _type == EntityTypes::LIGHT) - { - this->entityName = _entityName; - } + // Set to scoped name + this->entityName = _entityName; QMenu menu; @@ -283,16 +276,28 @@ void ModelRightMenu::OnApplyWrench() } std::string modelName, linkName; - if (vis == vis->GetRootVisual()) + if (vis->GetType() == rendering::Visual::VT_MODEL) { modelName = this->entityName; // If model selected just take the first link - linkName = vis->GetChild(0)->Name(); + for (unsigned int i = 0; i < vis->GetChildCount(); ++i) + { + rendering::VisualPtr currentChild = vis->GetChild(i); + if (currentChild->GetType() == rendering::Visual::VT_LINK) + { + linkName = currentChild->Name(); + break; + } + } } else { - modelName = vis->GetRootVisual()->Name(); - linkName = this->entityName; + // Links should always have a parent + if (vis->GetParent() != nullptr) + { + modelName = vis->GetParent()->Name(); + linkName = this->entityName; + } } applyWrenchDialog->Init(modelName, linkName); diff --git a/worlds/nested_multilink_shape.world b/worlds/nested_multilink_shape.world new file mode 100644 index 0000000000..65b44e4f8e --- /dev/null +++ b/worlds/nested_multilink_shape.world @@ -0,0 +1,111 @@ + + + + + 0 0 0 + + + model://sun + + + 0 0 0.5 0 0 0 + + 1 0 0 0 0 0 + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + -1.5 0 0 0 0 1.57 + + + + 0.5 + + + + + + + 0.5 + + + + + + + + + + + 2 0 0.5 0 0 0 + + 1 3 0 0 0 0 + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + + -1.5 3 0 0 0 1.57 + + + + 0.5 + + + + + + + 0.5 + + + + + + + + + + +