3 ➡️ 4 (#946)

Signed-off-by: Louise Poubel <[email protected]>

examples/worlds/plot_3d.sdf

@@ -0,0 +1,152 @@
+<?xml version="1.0" ?>
+<!--
+  Demo the Plot3D plugin.
+
+  Start this world and see how the pendulum's link's paths are plotted in the 3D scene as it moves.
+
+  The plugin is instantiated twice, once for each link of the pendulum.
+
+  Try out different parameters in each instance of the plugin to see how the plot changes.
+-->
+<sdf version="1.6">
+  <world name="shapes">
+
+    <gui fullscreen="0">
+
+      <!-- 3D scene -->
+      <plugin filename="GzScene3D" name="3D View">
+        <ignition-gui>
+          <title>3D View</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="string" key="state">docked</property>
+        </ignition-gui>
+
+        <engine>ogre2</engine>
+        <scene>scene</scene>
+        <ambient_light>0.4 0.4 0.4</ambient_light>
+        <background_color>0.8 0.8 0.8</background_color>
+        <camera_pose>5 -1.5 3 0 0.37 2.8</camera_pose>
+      </plugin>
+
+      <!-- World control -->
+      <plugin filename="WorldControl" name="World control">
+        <ignition-gui>
+          <title>World control</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">72</property>
+          <property type="double" key="width">121</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="left" target="left"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <play_pause>true</play_pause>
+        <step>true</step>
+        <start_paused>true</start_paused>
+      </plugin>
+
+      <!-- World statistics -->
+      <plugin filename="WorldStats" name="World stats">
+        <ignition-gui>
+          <title>World stats</title>
+          <property type="bool" key="showTitleBar">false</property>
+          <property type="bool" key="resizable">false</property>
+          <property type="double" key="height">110</property>
+          <property type="double" key="width">290</property>
+          <property type="double" key="z">1</property>
+
+          <property type="string" key="state">floating</property>
+          <anchors target="3D View">
+            <line own="right" target="right"/>
+            <line own="bottom" target="bottom"/>
+          </anchors>
+        </ignition-gui>
+
+        <sim_time>true</sim_time>
+        <real_time>true</real_time>
+        <real_time_factor>true</real_time_factor>
+        <iterations>true</iterations>
+      </plugin>
+
+      <!-- Entity tree -->
+      <plugin filename="EntityTree" name="Entity tree">
+      </plugin>
+
+      <!-- Plot 3D: upper link -->
+      <plugin filename="Plot3D" name="Plot 3D">
+        <ignition-gui>
+          <title>Upper link plot</title>
+        </ignition-gui>
+        <entity_name>Double_pendulum::upper_link</entity_name>
+        <color>0 0 1</color>
+        <offset>0 0 1</offset>
+        <maximum_points>20</maximum_points>
+        <minimum_distance>0.1</minimum_distance>
+      </plugin>
+
+      <!-- Plot 3D: lower link -->
+      <plugin filename="Plot3D" name="Plot 3D">
+        <ignition-gui>
+          <title>Lower link plot</title>
+        </ignition-gui>
+        <entity_name>Double_pendulum::lower_link</entity_name>
+        <color>0 1 0</color>
+        <offset>0 0 1</offset>
+        <maximum_points>40</maximum_points>
+        <minimum_distance>0.3</minimum_distance>
+      </plugin>
+    </gui>
+
+    <light type="directional" name="sun">
+      <cast_shadows>true</cast_shadows>
+      <pose>0 0 10 0 0 0</pose>
+      <diffuse>0.8 0.8 0.8 1</diffuse>
+      <specular>0.2 0.2 0.2 1</specular>
+      <attenuation>
+        <range>1000</range>
+        <constant>0.9</constant>
+        <linear>0.01</linear>
+        <quadratic>0.001</quadratic>
+      </attenuation>
+      <direction>-0.5 0.1 -0.9</direction>
+    </light>
+
+    <model name="ground_plane">
+      <static>true</static>
+      <link name="link">
+        <collision name="collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>100 100</size>
+            </plane>
+          </geometry>
+          <material>
+            <ambient>0.8 0.8 0.8 1</ambient>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
+    </model>
+
+    <include>
+      <name>Double_pendulum</name>
+      <uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Double pendulum with base</uri>
+    </include>
+
+  </world>
+</sdf>

include/ignition/gazebo/Link.hh

@@ -185,8 +185,9 @@ namespace ignition
       /// body-fixed frame. If no offset is given, the velocity at the origin of
       /// the Link frame will be returned.
       /// \param[in] _ecm Entity-component manager.
-      /// \return Linear velocity of the link or nullopt if the link does not
-      /// have components components::WorldPose.
+      /// \return Linear velocity of the link or nullopt if the velocity checks
+      /// aren't enabled.
+      /// \sa EnableVelocityChecks
       public: std::optional<math::Vector3d> WorldLinearVelocity(
           const EntityComponentManager &_ecm) const;
 
@@ -195,28 +196,47 @@ namespace ignition
       /// \param[in] _ecm Entity-component manager.
       /// \param[in] _offset Offset of the point from the origin of the Link
       /// frame, expressed in the body-fixed frame.
-      /// \return Linear velocity of the point on the body or nullopt if the
-      /// link does not have components components::WorldPose,
-      /// components::WorldLinearVelocity and components::WorldAngularVelocity.
+      /// \return Linear velocity of the point on the body or nullopt if
+      /// velocity checks aren't enabled.
+      /// \sa EnableVelocityChecks
       public: std::optional<math::Vector3d> WorldLinearVelocity(
           const EntityComponentManager &_ecm,
           const math::Vector3d &_offset) const;
 
       /// \brief Get the angular velocity of the link in the world frame
       /// \param[in] _ecm Entity-component manager.
-      /// \return Angular velocity of the link or nullopt if the link does not
-      /// have a components::WorldAngularVelocity component.
+      /// \return Angular velocity of the link or nullopt if velocity checks
+      /// aren't enabled.
+      /// \sa EnableVelocityChecks
       public: std::optional<math::Vector3d> WorldAngularVelocity(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief By default, Gazebo will not report velocities for a link, so
+      /// functions like `WorldLinearVelocity` will return nullopt. This
+      /// function can be used to enable all velocity checks.
+      /// \param[in] _ecm Mutable reference to the ECM.
+      /// \param[in] _enable True to enable checks, false to disable. Defaults
+      /// to true.
+      public: void EnableVelocityChecks(EntityComponentManager &_ecm,
+          bool _enable = true) const;
+
       /// \brief Get the linear acceleration of the body in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Linear acceleration of the body in the world frame or nullopt
-      /// if the link does not have a components::WorldLinearAcceleration
-      /// component.
+      /// if acceleration checks aren't enabled.
+      /// \sa EnableAccelerationChecks
       public: std::optional<math::Vector3d> WorldLinearAcceleration(
           const EntityComponentManager &_ecm) const;
 
+      /// \brief By default, Gazebo will not report accelerations for a link, so
+      /// functions like `WorldLinearAcceleration` will return nullopt. This
+      /// function can be used to enable all acceleration checks.
+      /// \param[in] _ecm Mutable reference to the ECM.
+      /// \param[in] _enable True to enable checks, false to disable. Defaults
+      /// to true.
+      public: void EnableAccelerationChecks(EntityComponentManager &_ecm,
+          bool _enable = true) const;
+
       /// \brief Get the inertia matrix in the world frame.
       /// \param[in] _ecm Entity-component manager.
       /// \return Inertia matrix in world frame, returns nullopt if link

include/ignition/gazebo/Server.hh

@@ -19,6 +19,7 @@
 
 #include <cstdint>
 #include <memory>
+#include <optional>
 #include <string>
 #include <ignition/gazebo/config.hh>
 #include <ignition/gazebo/EntityComponentManager.hh>
@@ -218,6 +219,16 @@ namespace ignition
                   const SystemPluginPtr &_system,
                   const unsigned int _worldIndex = 0);
 
+      /// \brief Add a System to the server. The server must not be running when
+      /// calling this.
+      /// \param[in] _system System to be added
+      /// \param[in] _worldIndex Index of the world to add to.
+      /// \return Whether the system was added successfully, or std::nullopt
+      /// if _worldIndex is invalid.
+      public: std::optional<bool> AddSystem(
+                  const std::shared_ptr<System> &_system,
+                  const unsigned int _worldIndex = 0);
+
       /// \brief Get an Entity based on a name.
       /// \details If multiple entities with the same name exist, the first
       /// entity found will be returned.

include/ignition/gazebo/Util.hh

@@ -18,6 +18,7 @@
 #define IGNITION_GAZEBO_UTIL_HH_
 
 #include <string>
+#include <unordered_set>
 #include <vector>
 
 #include <ignition/math/Pose3.hh>
@@ -51,6 +52,34 @@ namespace ignition
       const EntityComponentManager &_ecm, const std::string &_delim = "/",
       bool _includePrefix = true);
 
+    /// \brief Helper function to get an entity given its scoped name.
+    /// The scope may start at any level by default. For example, in this
+    /// hierarchy:
+    ///
+    /// world_name
+    ///  model_name
+    ///    link_name
+    ///
+    /// All these names will return the link entity:
+    ///
+    /// * world_name::model_name::link_name
+    /// * model_name::link_name
+    /// * link_name
+    ///
+    /// \param[in] _scopedName Entity's scoped name.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \param[in] _relativeTo Entity that the scoped name is relative to. The
+    /// scoped name does not include the name of this entity. If not provided,
+    /// the scoped name could be relative to any entity.
+    /// \param[in] _delim Delimiter between names, defaults to "::", it can't
+    /// be empty.
+    /// \return All entities that match the scoped name and relative to
+    /// requirements, or an empty set otherwise.
+    std::unordered_set<Entity> IGNITION_GAZEBO_VISIBLE entitiesFromScopedName(
+      const std::string &_scopedName, const EntityComponentManager &_ecm,
+      Entity _relativeTo = kNullEntity,
+      const std::string &_delim = "::");
+
     /// \brief Generally, each entity will be of some specific high-level type,
     /// such as World, Sensor, Collision, etc, and one type only.
     /// The entity type is usually marked by having some component that
@@ -101,6 +130,12 @@ namespace ignition
     Entity IGNITION_GAZEBO_VISIBLE worldEntity(const Entity &_entity,
         const EntityComponentManager &_ecm);
 
+    /// \brief Get the first world entity that's found.
+    /// \param[in] _ecm Immutable reference to ECM.
+    /// \return World entity ID.
+    Entity IGNITION_GAZEBO_VISIBLE worldEntity(
+        const EntityComponentManager &_ecm);
+
     /// \brief Helper function to remove a parent scope from a given name.
     /// This removes the first name found before the delimiter.
     /// \param[in] _name Input name possibly generated by scopedName.
@@ -174,6 +209,34 @@ namespace ignition
         const EntityComponentManager &_ecm,
         bool _excludeWorld = true);
 
+    /// \brief Helper function to "enable" a component (i.e. create it if it
+    /// doesn't exist) or "disable" a component (i.e. remove it if it exists).
+    /// \param[in] _ecm Mutable reference to the ECM
+    /// \param[in] _entity Entity whose component is being enabled
+    /// \param[in] _enable True to enable (create), false to disable (remove).
+    /// Defaults to true.
+    /// \return True if a component was created or removed, false if nothing
+    /// changed.
+    template <class ComponentType>
+    bool IGNITION_GAZEBO_VISIBLE enableComponent(EntityComponentManager &_ecm,
+        Entity _entity, bool _enable = true)
+    {
+      bool changed{false};
+
+      auto exists = _ecm.Component<ComponentType>(_entity);
+      if (_enable && !exists)
+      {
+        _ecm.CreateComponent(_entity, ComponentType());
+        changed = true;
+      }
+      else if (!_enable && exists)
+      {
+        _ecm.RemoveComponent<ComponentType>(_entity);
+        changed = true;
+      }
+      return changed;
+    }
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"IGN_GAZEBO_RESOURCE_PATH"};
 

include/ignition/gazebo/gui/GuiRunner.hh

@@ -47,6 +47,7 @@ class IGNITION_GAZEBO_VISIBLE GuiRunner : public QObject
   public: ~GuiRunner() override;
 
   /// \brief Callback when a plugin has been added.
+  /// This function has no effect and is left here for ABI compatibility.
   /// \param[in] _objectName Plugin's object name.
   public slots: void OnPluginAdded(const QString &_objectName);
 

src/Link.cc

@@ -32,6 +32,7 @@
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/Visual.hh"
 #include "ignition/gazebo/components/WindMode.hh"
+#include "ignition/gazebo/Util.hh"
 
 #include "ignition/gazebo/Link.hh"
 
@@ -228,6 +229,22 @@ std::optional<math::Vector3d> Link::WorldAngularVelocity(
       this->dataPtr->id);
 }
 
+//////////////////////////////////////////////////
+void Link::EnableVelocityChecks(EntityComponentManager &_ecm, bool _enable)
+    const
+{
+  enableComponent<components::WorldLinearVelocity>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::WorldAngularVelocity>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::LinearVelocity>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::AngularVelocity>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::WorldPose>(_ecm, this->dataPtr->id,
+      _enable);
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Vector3d> Link::WorldLinearAcceleration(
     const EntityComponentManager &_ecm) const
@@ -236,6 +253,16 @@ std::optional<math::Vector3d> Link::WorldLinearAcceleration(
       this->dataPtr->id);
 }
 
+//////////////////////////////////////////////////
+void Link::EnableAccelerationChecks(EntityComponentManager &_ecm, bool _enable)
+    const
+{
+  enableComponent<components::WorldLinearAcceleration>(_ecm, this->dataPtr->id,
+      _enable);
+  enableComponent<components::LinearAcceleration>(_ecm, this->dataPtr->id,
+      _enable);
+}
+
 //////////////////////////////////////////////////
 std::optional<math::Matrix3d> Link::WorldInertiaMatrix(
     const EntityComponentManager &_ecm) const