Partially fixes #1718 - Apply Force/Torque for nested models (#3039) (#…

…3053)

* Fixed ApplyWrenchDialog to properly handle nested models.

* Fix improper link being selected when opening applywrenchdialog

* Fixed styling issues

* Test nested models in ApplyWrenchDialog + update test world

* Separate nested_multilink world to avoid breaking ModelListWidget test

* Fix incorrect comment

* More styling fixes

* Fix link name parsing to support sdformat<11

Co-authored-by: Cameron Miller <camm73@yahoo.com>

gazebo/gui/ApplyWrenchDialog.cc

@@ -486,14 +486,29 @@ 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:
     // uint32_t flags = childVis->GetVisibilityFlags();
     // 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)
   {