Model Editor: Add Joints to model (#1196)

* Model Editor: Add Joints to model Signed-off-by: Michael Carroll <[email protected]> * Lint Signed-off-by: Michael Carroll <[email protected]> * Style and documentation Signed-off-by: Nate Koenig <[email protected]> * Suppress physics warnings on newly-created joints Signed-off-by: Michael Carroll <[email protected]> * Added a header Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
gazebosim · Dec 1, 2021 · d392d0f · d392d0f
1 parent 65cc189
commit d392d0f
Show file tree

Hide file tree

Showing 8 changed files with 419 additions and 59 deletions.
diff --git a/include/ignition/gazebo/SdfEntityCreator.hh b/include/ignition/gazebo/SdfEntityCreator.hh
@@ -122,6 +122,13 @@ namespace ignition
       /// \return Joint entity.
       public: Entity CreateEntities(const sdf::Joint *_joint);
 
+      /// \brief Create all entities that exist in the sdf::Joint object and
+      /// load their plugins.
+      /// \param[in] _joint SDF joint object.
+      /// \param[in] _resolved True if all frames are already resolved
+      /// \return Joint entity.
+      public: Entity CreateEntities(const sdf::Joint *_joint, bool _resolved);
+
       /// \brief Create all entities that exist in the sdf::Visual object and
       /// load their plugins.
       /// \param[in] _visual SDF visual object.

diff --git a/include/ignition/gazebo/gui/GuiEvents.hh b/include/ignition/gazebo/gui/GuiEvents.hh
@@ -18,6 +18,7 @@
 #define IGNITION_GAZEBO_GUI_GUIEVENTS_HH_
 
 #include <QEvent>
+#include <QMap>
 #include <QString>
 
 #include <set>
@@ -191,8 +192,8 @@ namespace events
     /// \brief Constructor
     /// \param[in] _tranformModeActive is the transform control mode active
     public: explicit ModelEditorAddEntity(QString _entity, QString _type,
-                ignition::gazebo::Entity _parent, QString _uri) :
-      QEvent(kType), entity(_entity), type(_type), parent(_parent), uri(_uri)
+                ignition::gazebo::Entity _parent) :
+      QEvent(kType), entity(_entity), type(_type), parent(_parent)
     {
     }
 
@@ -202,12 +203,6 @@ namespace events
       return this->entity;
     }
 
-    /// \brief Get the URI, if any, associated with the entity to add
-    public: QString Uri() const
-    {
-      return this->uri;
-    }
-
     /// \brief Get the entity type
     public: QString EntityType() const
     {
@@ -220,13 +215,12 @@ namespace events
       return this->parent;
     }
 
-    /// \brief Unique type for this event.
     static const QEvent::Type kType = QEvent::Type(QEvent::User + 7);
 
+    public : QMap<QString, QString> data;
     private: QString entity;
     private: QString type;
     private: ignition::gazebo::Entity parent;
-    private: QString uri;
   };
 
 }  // namespace events

diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc
@@ -588,6 +588,13 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Link *_link)
 
 //////////////////////////////////////////////////
 Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint)
+{
+  return this->CreateEntities(_joint, false);
+}
+
+//////////////////////////////////////////////////
+Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint,
+    bool _resolved)
 {
   IGN_PROFILE("SdfEntityCreator::CreateEntities(sdf::Joint)");
 
@@ -645,34 +652,54 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Joint *_joint)
   this->dataPtr->ecm->CreateComponent(jointEntity ,
       components::ThreadPitch(_joint->ThreadPitch()));
 
+
   std::string resolvedParentLinkName;
-  const auto resolveParentErrors =
-    _joint->ResolveParentLink(resolvedParentLinkName);
-  if (!resolveParentErrors.empty())
+  if (_resolved)
   {
-    ignerr << "Failed to resolve parent link for joint '" << _joint->Name()
-           << "' with parent name '" << _joint->ParentLinkName() << "'"
-           << std::endl;
+    resolvedParentLinkName = _joint->ParentLinkName();
+  }
+  else
+  {
+
+    const auto resolveParentErrors =
+      _joint->ResolveParentLink(resolvedParentLinkName);
+    if (!resolveParentErrors.empty())
+    {
+      ignerr << "Failed to resolve parent link for joint '" << _joint->Name()
+             << "' with parent name '" << _joint->ParentLinkName() << "'"
+             << std::endl;
+      for (const auto &error : resolveParentErrors)
+      {
+        ignerr << error << std::endl;
+      }
 
-    return kNullEntity;
+      return kNullEntity;
+    }
   }
   this->dataPtr->ecm->CreateComponent(
       jointEntity, components::ParentLinkName(resolvedParentLinkName));
 
   std::string resolvedChildLinkName;
-  const auto resolveChildErrors =
-    _joint->ResolveChildLink(resolvedChildLinkName);
-  if (!resolveChildErrors.empty())
-  {
-    ignerr << "Failed to resolve child link for joint '" << _joint->Name()
-           << "' with child name '" << _joint->ChildLinkName() << "'"
-           << std::endl;
-    for (const auto &error : resolveChildErrors)
+  if (_resolved)
+  {
+    resolvedChildLinkName = _joint->ChildLinkName();
+  }
+  else
+  {
+    const auto resolveChildErrors =
+      _joint->ResolveChildLink(resolvedChildLinkName);
+    if (!resolveChildErrors.empty())
     {
-      ignerr << error << std::endl;
-    }
+      ignerr << "Failed to resolve child link for joint '" << _joint->Name()
+             << "' with child name '" << _joint->ChildLinkName() << "'"
+             << std::endl;
+      for (const auto &error : resolveChildErrors)
+      {
+        ignerr << error << std::endl;
+      }
 
-    return kNullEntity;
+      return kNullEntity;
+    }
   }
 
   this->dataPtr->ecm->CreateComponent(

diff --git a/src/gui/plugins/component_inspector/ComponentInspector.cc b/src/gui/plugins/component_inspector/ComponentInspector.cc
@@ -103,6 +103,9 @@ namespace ignition::gazebo
     /// \brief Nested model or not
     public: bool nestedModel = false;
 
+    /// \brief If a model, keep track of available links
+    public: QStringList modelLinks = {};
+
     /// \brief Whether currently locked on a given entity
     public: bool locked{false};
 
@@ -497,6 +500,25 @@ void ComponentInspector::Update(const UpdateInfo &_info,
       }
       this->NestedModelChanged();
 
+      // Get available links for the model.
+      this->dataPtr->modelLinks.clear();
+      this->dataPtr->modelLinks.append("world");
+      _ecm.EachNoCache<
+        components::Name,
+        components::Link,
+        components::ParentEntity>([&](const ignition::gazebo::Entity &,
+              const components::Name *_name,
+              const components::Link *,
+              const components::ParentEntity *_parent) -> bool
+            {
+              if (_parent->Data() == this->dataPtr->entity)
+              {
+                this->dataPtr->modelLinks.push_back(
+                    QString::fromStdString(_name->Data()));
+              }
+              return true;
+            });
+      this->ModelLinksChanged();
       continue;
     }
 
@@ -1125,14 +1147,43 @@ bool ComponentInspector::NestedModel() const
   return this->dataPtr->nestedModel;
 }
 
+/////////////////////////////////////////////////
+void ComponentInspector::SetModelLinks(const QStringList &_modelLinks)
+{
+  this->dataPtr->modelLinks = _modelLinks;
+  this->ModelLinksChanged();
+}
+
+/////////////////////////////////////////////////
+QStringList ComponentInspector::ModelLinks() const
+{
+  return this->dataPtr->modelLinks;
+}
+
 /////////////////////////////////////////////////
 void ComponentInspector::OnAddEntity(const QString &_entity,
     const QString &_type)
 {
   // currently just assumes parent is the model
   // todo(anyone) support adding visuals / collisions / sensors to links
   ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent(
-      _entity, _type, this->dataPtr->entity, QString(""));
+      _entity, _type, this->dataPtr->entity);
+
+  ignition::gui::App()->sendEvent(
+      ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
+      &addEntityEvent);
+}
+
+/////////////////////////////////////////////////
+void ComponentInspector::OnAddJoint(const QString &_jointType,
+                                    const QString &_parentLink,
+                                    const QString &_childLink)
+{
+  ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent(
+      _jointType, "joint", this->dataPtr->entity);
+
+  addEntityEvent.data.insert("parent_link", _parentLink);
+  addEntityEvent.data.insert("child_link", _childLink);
 
   ignition::gui::App()->sendEvent(
       ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
@@ -1157,7 +1208,10 @@ void ComponentInspector::OnLoadMesh(const QString &_entity,
     }
 
     ignition::gazebo::gui::events::ModelEditorAddEntity addEntityEvent(
-        _entity, _type, this->dataPtr->entity, QString(meshStr.c_str()));
+        _entity, _type, this->dataPtr->entity);
+
+    addEntityEvent.data.insert("uri", QString(meshStr.c_str()));
+
     ignition::gui::App()->sendEvent(
         ignition::gui::App()->findChild<ignition::gui::MainWindow *>(),
         &addEntityEvent);

diff --git a/src/gui/plugins/component_inspector/ComponentInspector.hh b/src/gui/plugins/component_inspector/ComponentInspector.hh
@@ -185,6 +185,14 @@ namespace gazebo
       NOTIFY TypeChanged
     )
 
+    /// \brief Type
+    Q_PROPERTY(
+      QStringList modelLinks
+      READ ModelLinks
+      WRITE SetModelLinks
+      NOTIFY ModelLinksChanged
+    )
+
     /// \brief Locked
     Q_PROPERTY(
       bool locked
@@ -355,6 +363,25 @@ namespace gazebo
     public: Q_INVOKABLE void OnAddEntity(const QString &_entity,
                 const QString &_type);
 
+    /// \brief Callback in Qt thread when a joint is to be added
+    /// \param[in] _jointType Type of joint to add (revolute, fixed, etc)
+    /// \param[in] _parentLink Name of the link to be the parent link
+    /// \param[in] _childLink Name of the link to be the child link
+    public: Q_INVOKABLE void OnAddJoint(const QString &_jointType,
+                const QString &_parentLink,
+                const QString &_childLink);
+
+    /// \brief Return the list of availabe links if a model is selected.
+    /// \return List of available links.
+    public: Q_INVOKABLE QStringList ModelLinks() const;
+
+    /// \brief Set the list of availabe links when a model is selected.
+    /// \param[in] _modelLinks List of available links.
+    public: Q_INVOKABLE void SetModelLinks(const QStringList &_modelLinks);
+
+    /// \brief Notify that locked has changed.
+    signals: void ModelLinksChanged();
+
     /// \brief Callback to insert a new entity
     /// \param[in] _entity Entity to add, e.g. box, sphere, cylinder, etc
     /// \param[in] _type Entity type, e.g. link, visual, collision, etc