Merge pull request #3062 from osrf/backport_3031

Backport #3031 - Make links within nested models modifiable from GUI Client

gazebo/gui/ModelListWidget.cc

@@ -407,7 +407,9 @@ void ModelListWidget::ProcessModelMsgs()
         for (int i = 0; i < (*iter).link_size(); ++i)
         {
           std::string linkName = (*iter).link(i).name();
-          int index = linkName.rfind("::") + 2;
+
+          // get unscoped name by stripping parent
+          int index = linkName.find(name) + name.length() + 2;
           std::string linkNameShort = linkName.substr(index,
                                                       linkName.size() - index);
 
@@ -436,7 +438,8 @@ void ModelListWidget::ProcessModelMsgs()
         {
           std::string jointName = (*iter).joint(i).name();
 
-          int index = jointName.rfind("::") + 2;
+          // get unscoped name by stripping parent
+          int index = jointName.find(name) + name.length() + 2;
           std::string jointNameShort = jointName.substr(
               index, jointName.size() - index);
 
@@ -1117,9 +1120,6 @@ void ModelListWidget::ModelPropertyChanged(QtProperty *_item)
     msgs::Link *linkMsg = msg.add_link();
     linkMsg->set_id(this->dataPtr->linkMsg.id());
     std::string linkName = this->dataPtr->linkMsg.name();
-    size_t index = linkName.find_last_of("::");
-    if (index != std::string::npos)
-      linkName = linkName.substr(index+1);
     linkMsg->set_name(linkName);
     fillMsg = linkMsg;
   }

gazebo/gui/ModelListWidget_TEST.cc

@@ -16,6 +16,7 @@
 */
 #include <boost/filesystem.hpp>
 #include <memory>
+#include "gazebo/common/SystemPaths.hh"
 #include "gazebo/msgs/msgs.hh"
 #include "gazebo/transport/TransportIface.hh"
 #include "gazebo/gui/GuiEvents.hh"
@@ -24,7 +25,6 @@
 #include "gazebo/gui/qtpropertybrowser/qtvariantproperty.h"
 #include "gazebo/gui/ModelListWidget.hh"
 #include "gazebo/gui/ModelListWidget_TEST.hh"
-
 #include "test_config.h"
 
 /////////////////////////////////////////////////
@@ -70,13 +70,13 @@ void ModelListWidget_TEST::TreeWidget()
 /////////////////////////////////////////////////
 void ModelListWidget_TEST::OnResponse(ConstResponsePtr &_msg)
 {
-  gazebo::msgs::Model_V modelVMsg;
-  if (_msg->has_type() && _msg->type() == modelVMsg.GetTypeName())
+  gazebo::msgs::Scene sceneMsg;
+  if (_msg->has_type() && _msg->type() == sceneMsg.GetTypeName())
   {
-    modelVMsg.ParseFromString(_msg->serialized_data());
-    for (int i = 0; i < modelVMsg.models_size(); i++)
+    sceneMsg.ParseFromString(_msg->serialized_data());
+    for (int i = 0; i < sceneMsg.model_size(); i++)
     {
-      gazebo::gui::Events::modelUpdate(modelVMsg.models(i));
+      gazebo::gui::Events::modelUpdate(sceneMsg.model(i));
     }
   }
 }
@@ -310,7 +310,7 @@ void ModelListWidget_TEST::ModelsTree()
       &ModelListWidget_TEST::OnResponse, this);
 
   gazebo::msgs::Request *requestMsg =
-      gazebo::msgs::CreateRequest("entity_list");
+      gazebo::msgs::CreateRequest("scene_info");
   requestPub->Publish(*requestMsg);
 
   // Get tree widget
@@ -397,7 +397,7 @@ void ModelListWidget_TEST::ModelProperties()
       &ModelListWidget_TEST::OnResponse, this);
 
   gazebo::msgs::Request *requestMsg =
-      gazebo::msgs::CreateRequest("entity_list");
+      gazebo::msgs::CreateRequest("scene_info");
   requestPub->Publish(*requestMsg);
 
   // Get tree widget
@@ -650,15 +650,20 @@ void ModelListWidget_TEST::ModelProperties()
 }
 
 /////////////////////////////////////////////////
-void ModelListWidget_TEST::LinkProperties()
+void ModelListWidget_TEST::LinkProperties(const std::string &_worldFilename,
+    const std::string &_modelName, int _nestLevel)
 {
   gazebo::gui::ModelListWidget *modelListWidget
       = new gazebo::gui::ModelListWidget;
   modelListWidget->show();
   modelListWidget->setGeometry(0, 0, 400, 800);
   QCoreApplication::processEvents();
 
-  this->Load("worlds/multilink_shape.world");
+  // Add the test model database
+  gazebo::common::SystemPaths::Instance()->AddModelPathsUpdate(
+      PROJECT_SOURCE_PATH "/test/models");
+
+  this->Load(_worldFilename);
 
   gazebo::transport::NodePtr node;
   node = gazebo::transport::NodePtr(new gazebo::transport::Node());
@@ -669,7 +674,7 @@ void ModelListWidget_TEST::LinkProperties()
       &ModelListWidget_TEST::OnResponse, this);
 
   gazebo::msgs::Request *requestMsg =
-      gazebo::msgs::CreateRequest("entity_list");
+      gazebo::msgs::CreateRequest("scene_info");
   requestPub->Publish(*requestMsg);
 
   // Get tree widget
@@ -683,8 +688,8 @@ void ModelListWidget_TEST::LinkProperties()
   QTreeWidgetItem *modelsItem = treeModelItems.front();
   QVERIFY(modelsItem != nullptr);
 
-  // verify that there is only 1 model
-  int modelCount = 1;
+  // verify that there are the models
+  int modelCount = _nestLevel + 1;
   int maxSleep = 10;
   int sleep = 0;
   while (modelsItem->childCount() < modelCount && sleep < maxSleep)
@@ -696,10 +701,9 @@ void ModelListWidget_TEST::LinkProperties()
   QVERIFY(sleep < maxSleep);
 
   // Get the model item
-  QTreeWidgetItem *modelItem = modelsItem->child(0);
+  QTreeWidgetItem *modelItem = modelsItem->child(_nestLevel);
   QVERIFY(modelItem != nullptr);
-  std::string modelName = "multilink";
-  QCOMPARE(modelItem->text(0), tr(modelName.c_str()));
+  QCOMPARE(modelItem->text(0), tr(_modelName.c_str()));
 
   // Get propery browser widget
   QObject *propTreeObj =
@@ -784,13 +788,13 @@ void ModelListWidget_TEST::LinkProperties()
 
   // check the box link properties
   this->CheckLinkProperty(propTreeBrowser->properties(),
-      modelName + "::" + boxLinkName, false, true, false, true, false,
+      _modelName + "::" + boxLinkName, false, true, false, true, false,
       ignition::math::Pose3d(1.0, 0, 0, 0, 0, 0));
 
   // change box link properties
   // TODO changing link name currently fails.
   this->SetLinkProperty(propTreeBrowser, propTreeBrowser->properties(),
-      modelName + "::" + boxLinkName, true, false, true, true, false,
+      _modelName + "::" + boxLinkName, true, false, true, true, false,
       ignition::math::Pose3d(2.5, 1.0, 4.2, 0.8, 0.5, 0.1));
 
   // select the box link again to refresh the property browser
@@ -822,7 +826,7 @@ void ModelListWidget_TEST::LinkProperties()
   // verify the link properties are sucessfully set
   // the link is canonical so the pose should remain the same
   this->CheckLinkProperty(propTreeBrowser->properties(),
-      modelName + "::" + boxLinkName, true, false, true, true, false,
+      _modelName + "::" + boxLinkName, true, false, true, true, false,
       ignition::math::Pose3d(1.0, 0, 0, 0, 0, 0));
 
   // select the sphere link
@@ -859,13 +863,13 @@ void ModelListWidget_TEST::LinkProperties()
 
   // check the sphere link properties
   this->CheckLinkProperty(propTreeBrowser->properties(),
-      modelName + "::" + sphereLinkName, false, true, false, false, false,
+      _modelName + "::" + sphereLinkName, false, true, false, false, false,
       ignition::math::Pose3d(-1.5, 0, 0, 0, 0, 1.57));
 
   // change sphere link properties
   // TODO changing link name currently fails.
   this->SetLinkProperty(propTreeBrowser, propTreeBrowser->properties(),
-      modelName + "::" + sphereLinkName, true, false, true, false, false,
+      _modelName + "::" + sphereLinkName, true, false, true, false, false,
       ignition::math::Pose3d(-2.0, 0.1, -1.2, 0, 1.57, 0));
 
   // select the sphere link again to refresh the property browser
@@ -894,7 +898,7 @@ void ModelListWidget_TEST::LinkProperties()
 
   // verify the link properties are sucessfully set
   this->CheckLinkProperty(propTreeBrowser->properties(),
-      modelName + "::" + sphereLinkName, true, false, true, false, false,
+      _modelName + "::" + sphereLinkName, true, false, true, false, false,
       ignition::math::Pose3d(-2.0, 0.1, -1.2, 0, 1.57, 0));
 
   modelListWidget->hide();
@@ -903,6 +907,24 @@ void ModelListWidget_TEST::LinkProperties()
   delete modelListWidget;
 }
 
+/////////////////////////////////////////////////
+void ModelListWidget_TEST::LinkProperties()
+{
+  LinkProperties("worlds/multilink_shape.world", "multilink", 0);
+}
+
+/////////////////////////////////////////////////
+void ModelListWidget_TEST::IncludedLinkProperties()
+{
+  LinkProperties("worlds/multilink_shape_included.world", "multilink", 0);
+}
+
+/////////////////////////////////////////////////
+void ModelListWidget_TEST::NestedLinkProperties()
+{
+  LinkProperties("worlds/multilink_shape_nested.world",
+      "multilink_nested::multilink", 1);
+}
 
 /////////////////////////////////////////////////
 void ModelListWidget_TEST::PluginProperties()
@@ -926,7 +948,7 @@ void ModelListWidget_TEST::PluginProperties()
 
   // Request list of entities
   gazebo::msgs::Request *requestMsg =
-      gazebo::msgs::CreateRequest("entity_list");
+      gazebo::msgs::CreateRequest("scene_info");
   requestPub->Publish(*requestMsg);
 
   // Get tree widget
@@ -992,11 +1014,22 @@ void ModelListWidget_TEST::PluginProperties()
   maxSleep = 5;
   while (!modelItem->isSelected() && sleep < maxSleep)
   {
-    QTest::qWait(10);
+    QTest::qWait(500);
     sleep++;
   }
   QVERIFY(modelItem->isSelected());
 
+  // Wait for the plugin properties to appear
+  sleep = 0;
+  maxSleep = 10;
+  while (propTreeBrowser->properties().size() == 0 && sleep < maxSleep)
+  {
+    QCoreApplication::processEvents();
+    QTest::qWait(500);
+    sleep++;
+  }
+  auto propertySize = propTreeBrowser->properties().size();
+
   // Get the buoyancy plugin
   QTreeWidgetItem *pluginItem = modelItem->child(6);
   QVERIFY(pluginItem != nullptr);
@@ -1022,7 +1055,9 @@ void ModelListWidget_TEST::PluginProperties()
   // Wait for the plugin properties to appear
   sleep = 0;
   maxSleep = 10;
-  while (propTreeBrowser->properties().size() == 0 && sleep < maxSleep)
+  while (propTreeBrowser->properties().size() == 0 &&
+      propTreeBrowser->properties().size() != propertySize &&
+      sleep < maxSleep)
   {
     QCoreApplication::processEvents();
     QTest::qWait(500);

gazebo/gui/ModelListWidget_TEST.hh

@@ -112,8 +112,34 @@ class ModelListWidget_TEST : public QTestFixture
   /// This is similar to the ModelProperties test except the property browser
   /// now only displays link properties as the result of directly clicking on
   /// the link item in the models tree widget.
+  /// \param[in] _worldFilename File name of world to be loaded
+  /// \param[in] _modelName name of the model whose link properties are being
+  /// tested
+  /// \param[in] _nestLevel the level the child is nested at
+  private slots: void LinkProperties(const std::string &_worldFilename,
+      const std::string &_modelName, int _nestLevel);
+
+  /// \brief Test that the property browser displays correct link properties
+  /// in a world with an unnested model defined outright.
+  /// This is similar to the ModelProperties test except the property browser
+  /// now only displays link properties as the result of directly clicking on
+  /// the link item in the models tree widget.
   private slots: void LinkProperties();
 
+  /// \brief Test that the property browser displays correct link properties
+  /// in a world with an unnested model referenced in an include.
+  /// This is similar to the ModelProperties test except the property browser
+  /// now only displays link properties as the result of directly clicking on
+  /// the link item in the models tree widget.
+  private slots: void IncludedLinkProperties();
+
+  /// \brief Test that the property browser displays correct link properties
+  /// in a world with nested models.
+  /// This is similar to the ModelProperties test except the property browser
+  /// now only displays link properties as the result of directly clicking on
+  /// the link item in the models tree widget.
+  private slots: void NestedLinkProperties();
+
   /// \brief Test that the property browser displays correct plugin properties.
   /// This is similar to the LinkProperties test.
   private slots: void PluginProperties();

gazebo/physics/Base.cc

@@ -291,6 +291,26 @@ BasePtr Base::GetById(unsigned int _id) const
   return result;
 }
 
+//////////////////////////////////////////////////
+BasePtr Base::GetByIdRecursive(unsigned int _id)
+{
+  if (this->GetId() == _id)
+  {
+    return shared_from_this();
+  }
+
+  BasePtr result;
+  Base_V::const_iterator biter;
+
+  for (biter = this->children.begin();
+      biter != this->children.end() && result == nullptr; ++biter)
+  {
+    result = (*biter)->GetByIdRecursive(_id);
+  }
+
+  return result;
+}
+
 //////////////////////////////////////////////////
 BasePtr Base::GetByName(const std::string &_name)
 {
@@ -316,6 +336,40 @@ std::string Base::GetScopedName(bool _prependWorldName) const
     return this->scopedName;
 }
 
+//////////////////////////////////////////////////
+std::string Base::StripScopedName(const std::string &_name) const
+{
+  if (_name.find(this->GetScopedName() + "::") == 0)
+  {
+    return _name.substr(this->GetScopedName().size() + 2);
+  }
+  else
+  {
+    // it's okay if current element IS the world, otherwise logerr
+    if ((this->GetName() != this->world->Name()))
+    {
+      gzerr << "Cannot strip scoped name " << this->GetScopedName()
+            << " from the beginning of given _name " << _name
+            << ". Returning _name without stripping."
+            << std::endl;
+    }
+    return _name;
+  }
+}
+
+//////////////////////////////////////////////////
+std::string Base::StripParentScopedName(const std::string &_name) const
+{
+  if (!this->GetParent())
+  {
+    return _name;
+  }
+  else
+  {
+    return this->GetParent()->StripScopedName(_name);
+  }
+}
+
 //////////////////////////////////////////////////
 common::URI Base::URI() const
 {

gazebo/physics/Base.hh

@@ -226,6 +226,13 @@ namespace gazebo
       public: BasePtr GetById(unsigned int _id) const;
       /// \endcond
 
+      /// This is an internal function.
+      /// \brief Recursively get descendent or self by id.
+      /// \param[in] _id ID of the object to retreive.
+      /// \return A pointer to the object, NULL if not found
+      public: BasePtr GetByIdRecursive(unsigned int _id);
+
+
       /// \brief Get by name.
       /// \param[in] _name Get a child (or self) object by name
       /// \return A pointer to the object, NULL if not found
@@ -276,6 +283,18 @@ namespace gazebo
       /// \return The scoped name.
       public: std::string GetScopedName(bool _prependWorldName = false) const;
 
+      /// \brief Return a short version of the name with "ScopedName::" removed
+      /// \param[in] _scoped name - Usually the scoped name of a new
+      /// name a child entity is to be set to.
+      /// \return The stripped name.
+      public: std::string StripScopedName(const std::string &_name) const;
+
+      /// \brief Return a short version of the name with "ParentScopedName::" removed
+      /// \param[in] _scoped name - Usually the scoped name of a new
+      /// name this entity is to be set to.
+      /// \return The stripped name.
+      public: std::string StripParentScopedName(const std::string &_name) const;
+
       /// \brief Return the common::URI of this entity.
       /// The URI includes the world where the entity is contained and all the
       /// hierarchy of sub-entities that can compose this entity.