Added recreate and resolved conflicts

Signed-off-by: Nate Koenig <[email protected]>

examples/worlds/sensors.sdf

@@ -183,6 +183,30 @@
             </z>
           </magnetometer>
         </sensor>
+        <sensor name="laser" type="gpu_ray">
+          <pose>0.43 0 0.26 0 0 0</pose>
+          <update_rate>20</update_rate>
+          <lidar>
+            <scan>
+              <horizontal>
+                <samples>720</samples>
+                <resolution>1</resolution>
+                <min_angle>-2.3562</min_angle>
+                <max_angle>2.3562</max_angle>
+              </horizontal>
+            </scan>
+            <range>
+              <min>0.04</min>
+              <max>5</max>
+              <resolution>0.01</resolution>
+            </range>
+            <noise>
+              <type>gaussian</type>
+              <mean>0</mean>
+              <stddev>0.01</stddev>
+            </noise>
+          </lidar>
+        </sensor>
       </link>
     </model>
 

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.

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

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(

src/cmd/ModelCommandAPI.cc

@@ -25,6 +25,7 @@
 #include <sdf/Altimeter.hh>
 #include <sdf/Camera.hh>
 #include <sdf/Imu.hh>
+#include <sdf/Lidar.hh>
 #include <sdf/Magnetometer.hh>
 #include <sdf/Noise.hh>
 #include <sdf/Sensor.hh>
@@ -39,6 +40,7 @@
 #include <ignition/gazebo/components/Altimeter.hh>
 #include <ignition/gazebo/components/Camera.hh>
 #include <ignition/gazebo/components/ChildLinkName.hh>
+#include <ignition/gazebo/components/GpuLidar.hh>
 #include <ignition/gazebo/components/Imu.hh>
 #include <ignition/gazebo/components/Inertial.hh>
 #include <ignition/gazebo/components/Joint.hh>
@@ -459,6 +461,51 @@ void printImu(const uint64_t _entity, const EntityComponentManager &_ecm,
     << "- Orientation enabled:" << imu->OrientationEnabled() << std::endl;
 }
 
+//////////////////////////////////////////////////
+void printGpuLidar(const uint64_t _entity,
+    const EntityComponentManager &_ecm, int _spaces)
+{
+  // Get the type and return if the _entity does not have the correct
+  // component.
+  auto comp = _ecm.Component<components::GpuLidar>(_entity);
+  if (!comp)
+    return;
+
+  const sdf::Sensor &sensor = comp->Data();
+  const sdf::Lidar *lidar = sensor.LidarSensor();
+
+  std::cout << std::string(_spaces, ' ') << "- Range:\n";
+  std::cout << std::string(_spaces+2, ' ') << "- Min: "
+    << lidar->RangeMin() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Max: "
+    << lidar->RangeMax() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Resolution: "
+    << lidar->RangeResolution() << std::endl;
+
+  std::cout << std::string(_spaces, ' ') << "- Horizontal scan:\n";
+  std::cout << std::string(_spaces+2, ' ') << "- Samples: "
+    << lidar->HorizontalScanSamples() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Resolution: "
+    << lidar->HorizontalScanResolution() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Min angle: "
+    << lidar->HorizontalScanMinAngle() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Max angle: "
+    << lidar->HorizontalScanMaxAngle() << std::endl;
+
+  std::cout << std::string(_spaces, ' ') << "- Vertical scan:\n";
+  std::cout << std::string(_spaces+2, ' ') << "- Samples: "
+    << lidar->VerticalScanSamples() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Resolution: "
+    << lidar->VerticalScanResolution() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Min angle: "
+    << lidar->VerticalScanMinAngle() << std::endl;
+  std::cout << std::string(_spaces+2, ' ') << "- Max angle: "
+    << lidar->VerticalScanMaxAngle() << std::endl;
+
+  std::cout << std::string(_spaces, ' ') << "- Noise:\n";
+  printNoise(lidar->LidarNoise(), _spaces + 2);
+}
+
 //////////////////////////////////////////////////
 void printMagnetometer(const uint64_t _entity,
     const EntityComponentManager &_ecm, int _spaces)
@@ -633,6 +680,7 @@ void printLinks(const uint64_t _modelEntity,
       printAirPressure(sensor, _ecm, spaces + 2);
       printAltimeter(sensor, _ecm, spaces + 2);
       printCamera(sensor, _ecm, spaces + 2);
+      printGpuLidar(sensor, _ecm, spaces + 2);
       printImu(sensor, _ecm, spaces + 2);
       printMagnetometer(sensor, _ecm, spaces + 2);
       printRgbdCamera(sensor, _ecm, spaces + 2);

src/gui/plugins/component_inspector/CMakeLists.txt

@@ -5,6 +5,7 @@ gz_add_gui_plugin(ComponentInspector
     ComponentInspector.cc
     Imu.cc
     JointType.cc
+    Lidar.cc
     Magnetometer.cc
     ModelEditor.cc
     Types.cc
@@ -14,6 +15,7 @@ gz_add_gui_plugin(ComponentInspector
     ComponentInspector.hh
     Imu.hh
     JointType.hh
+    Lidar.hh
     Magnetometer.hh
     ModelEditor.hh
     Types.hh

src/gui/plugins/component_inspector/ComponentInspector.cc

@@ -75,6 +75,7 @@
 #include "ComponentInspector.hh"
 #include "Imu.hh"
 #include "JointType.hh"
+#include "Lidar.hh"
 #include "Magnetometer.hh"
 #include "ModelEditor.hh"
 
@@ -103,6 +104,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};
 
@@ -130,6 +134,9 @@ namespace ignition::gazebo
     /// \brief Joint inspector elements
     public: std::unique_ptr<ignition::gazebo::JointType> joint;
 
+    /// \brief Lidar inspector elements
+    public: std::unique_ptr<ignition::gazebo::Lidar> lidar;
+
     /// \brief Magnetometer inspector elements
     public: std::unique_ptr<ignition::gazebo::Magnetometer> magnetometer;
 
@@ -458,6 +465,9 @@ void ComponentInspector::LoadConfig(const tinyxml2::XMLElement *)
   // Create the joint
   this->dataPtr->joint = std::make_unique<JointType>(this);
 
+  // Create the lidar
+  this->dataPtr->lidar = std::make_unique<Lidar>(this);
+
   // Create the magnetometer
   this->dataPtr->magnetometer = std::make_unique<Magnetometer>(this);
 }
@@ -497,6 +507,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 +1154,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 +1215,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);