Fixed IMU system plugin

src/systems/imu/Imu.cc

@@ -27,8 +27,6 @@
 
 #include <ignition/common/Profiler.hh>
 
-#include <ignition/transport/Node.hh>
-
 #include <ignition/sensors/SensorFactory.hh>
 #include <ignition/sensors/ImuSensor.hh>
 
@@ -58,8 +56,14 @@ class ignition::gazebo::systems::ImuPrivate
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief Keep track of world ID, which is equivalent to the scene's
+  /// root visual.
+  /// Defaults to zero, which is considered invalid by Ignition Gazebo.
   public: Entity worldEntity = kNullEntity;
 
+  /// \brief True if the rendering component is initialized
+  public: bool initialize = false;
+
   /// \brief Create IMU sensor
   /// \param[in] _ecm Mutable reference to ECM.
   public: void CreateImuEntities(EntityComponentManager &_ecm);
@@ -68,6 +72,17 @@ class ignition::gazebo::systems::ImuPrivate
   /// \param[in] _ecm Immutable reference to ECM.
   public: void Update(const EntityComponentManager &_ecm);
 
+  /// \brief Create sensor
+  /// \param[in] _ecm Mutable reference to ECM.
+  /// \param[in] _entity Entity of the IMU
+  /// \param[in] _imu IMU component.
+  /// \param[in] _parent Parent entity component.
+  public: void addIMU(
+    EntityComponentManager &_ecm,
+    const Entity _entity,
+    const components::Imu *_imu,
+    const components::ParentEntity *_parent);
+
   /// \brief Remove IMU sensors if their entities have been removed from
   /// simulation.
   /// \param[in] _ecm Immutable reference to ECM.
@@ -121,6 +136,63 @@ void Imu::PostUpdate(const UpdateInfo &_info,
   this->dataPtr->RemoveImuEntities(_ecm);
 }
 
+//////////////////////////////////////////////////
+void ImuPrivate::addIMU(
+  EntityComponentManager &_ecm,
+  const Entity _entity,
+  const components::Imu *_imu,
+  const components::ParentEntity *_parent)
+{
+  // Get the world acceleration (defined in world frame)
+  auto gravity = _ecm.Component<components::Gravity>(worldEntity);
+  if (nullptr == gravity)
+  {
+    ignerr << "World missing gravity." << std::endl;
+    return;
+  }
+
+  // create sensor
+  std::string sensorScopedName =
+      removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
+  sdf::Sensor data = _imu->Data();
+  data.SetName(sensorScopedName);
+  // check topic
+  if (data.Topic().empty())
+  {
+    std::string topic = scopedName(_entity, _ecm) + "/imu";
+    data.SetTopic(topic);
+  }
+  std::unique_ptr<sensors::ImuSensor> sensor =
+      this->sensorFactory.CreateSensor<
+      sensors::ImuSensor>(data);
+  if (nullptr == sensor)
+  {
+    ignerr << "Failed to create sensor [" << sensorScopedName << "]"
+           << std::endl;
+    return;
+  }
+
+  // set sensor parent
+  std::string parentName = _ecm.Component<components::Name>(
+      _parent->Data())->Data();
+  sensor->SetParent(parentName);
+
+  // set gravity - assume it remains fixed
+  sensor->SetGravity(gravity->Data());
+
+  // Get initial pose of sensor and set the reference z pos
+  // The WorldPose component was just created and so it's empty
+  // We'll compute the world pose manually here
+  math::Pose3d p = worldPose(_entity, _ecm);
+  sensor->SetOrientationReference(p.Rot());
+
+  // Set topic
+  _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
+
+  this->entitySensorMap.insert(
+      std::make_pair(_entity, std::move(sensor)));
+}
+
 //////////////////////////////////////////////////
 void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
 {
@@ -134,63 +206,31 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
     return;
   }
 
-  // Get the world acceleration (defined in world frame)
-  auto gravity = _ecm.Component<components::Gravity>(worldEntity);
-  if (nullptr == gravity)
+  if (!this->initialize)
   {
-    ignerr << "World missing gravity." << std::endl;
-    return;
-  }
-
-  // Create IMUs
-  _ecm.EachNew<components::Imu, components::ParentEntity>(
-    [&](const Entity &_entity,
-        const components::Imu *_imu,
-        const components::ParentEntity *_parent)->bool
-      {
-        // create sensor
-        std::string sensorScopedName =
-            removeParentScope(scopedName(_entity, _ecm, "::", false), "::");
-        sdf::Sensor data = _imu->Data();
-        data.SetName(sensorScopedName);
-        // check topic
-        if (data.Topic().empty())
+    // Create IMUs
+    _ecm.Each<components::Imu, components::ParentEntity>(
+      [&](const Entity &_entity,
+          const components::Imu *_imu,
+          const components::ParentEntity *_parent)->bool
         {
-          std::string topic = scopedName(_entity, _ecm) + "/imu";
-          data.SetTopic(topic);
-        }
-        std::unique_ptr<sensors::ImuSensor> sensor =
-            this->sensorFactory.CreateSensor<
-            sensors::ImuSensor>(data);
-        if (nullptr == sensor)
+          addIMU(_ecm, _entity, _imu, _parent);
+          return true;
+        });
+      this->initialize = true;
+  }
+  else
+  {
+    // Create IMUs
+    _ecm.EachNew<components::Imu, components::ParentEntity>(
+      [&](const Entity &_entity,
+          const components::Imu *_imu,
+          const components::ParentEntity *_parent)->bool
         {
-          ignerr << "Failed to create sensor [" << sensorScopedName << "]"
-                 << std::endl;
+          addIMU(_ecm, _entity, _imu, _parent);
           return true;
-        }
-
-        // set sensor parent
-        std::string parentName = _ecm.Component<components::Name>(
-            _parent->Data())->Data();
-        sensor->SetParent(parentName);
-
-        // set gravity - assume it remains fixed
-        sensor->SetGravity(gravity->Data());
-
-        // Get initial pose of sensor and set the reference z pos
-        // The WorldPose component was just created and so it's empty
-        // We'll compute the world pose manually here
-        math::Pose3d p = worldPose(_entity, _ecm);
-        sensor->SetOrientationReference(p.Rot());
-
-        // Set topic
-        _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
-
-        this->entitySensorMap.insert(
-            std::make_pair(_entity, std::move(sensor)));
-
-        return true;
-      });
+        });
+    }
 }
 
 //////////////////////////////////////////////////