Parse new param for enabling / disabling IMU orientation output

src/systems/imu/Imu.cc

@@ -58,6 +58,10 @@ class ignition::gazebo::systems::ImuPrivate
   /// \brief Ign-sensors sensor factory for creating sensors
   public: sensors::SensorFactory sensorFactory;
 
+  /// \brief True to compute and output orientation values,
+  /// false to leave the orientation field empty in the published msg.
+  public: bool outputOrientation = true;
+
   public: Entity worldEntity = kNullEntity;
 
   /// \brief Create IMU sensor
@@ -82,6 +86,19 @@ Imu::Imu() : System(), dataPtr(std::make_unique<ImuPrivate>())
 //////////////////////////////////////////////////
 Imu::~Imu() = default;
 
+//////////////////////////////////////////////////
+void Imu::Configure(const Entity & /*_entity*/,
+    const std::shared_ptr<const sdf::Element> &_sdf,
+    gazebo::EntityComponentManager & /*_ecm*/,
+    gazebo::EventManager & /*_eventMgr*/)
+{
+  if (_sdf->HasElement("output_orientation"))
+  {
+    this->dataPtr->outputOrientation =
+        _sdf->Get<bool>("output_orientation");
+  }
+}
+
 //////////////////////////////////////////////////
 void Imu::PreUpdate(const UpdateInfo &/*_info*/,
     EntityComponentManager &_ecm)
@@ -186,6 +203,9 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
         // Set topic
         _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
 
+        // Set whether to output orientation
+        sensor->SetOrientationEnabled(this->outputOrientation);
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 
@@ -254,7 +274,8 @@ void ImuPrivate::RemoveImuEntities(
 
 IGNITION_ADD_PLUGIN(Imu, System,
   Imu::ISystemPreUpdate,
-  Imu::ISystemPostUpdate
+  Imu::ISystemPostUpdate,
+  Imu::ISystemConfigure
 )
 
 IGNITION_ADD_PLUGIN_ALIAS(Imu, "ignition::gazebo::systems::Imu")

src/systems/imu/Imu.hh

@@ -40,14 +40,21 @@ namespace systems
   class Imu:
     public System,
     public ISystemPreUpdate,
-    public ISystemPostUpdate
+    public ISystemPostUpdate,
+    public ISystemConfigure
   {
     /// \brief Constructor
     public: explicit Imu();
 
     /// \brief Destructor
     public: ~Imu() override;
 
+    /// Documentation inherited
+    public: void Configure(const Entity &_entity,
+                           const std::shared_ptr<const sdf::Element> &_sdf,
+                           EntityComponentManager &_ecm,
+                           gazebo::EventManager &_eventMgr) final;
+
     /// Documentation inherited
     public: void PreUpdate(const UpdateInfo &_info,
                            EntityComponentManager &_ecm) final;

test/integration/imu_system.cc

@@ -212,3 +212,43 @@ TEST_F(ImuTest, ModelFalling)
   EXPECT_EQ(imuMsgs.back().entity_name(), scopedName);
   mutex.unlock();
 }
+
+/////////////////////////////////////////////////
+// The test checks to make sure orientation is not published if it is deabled
+TEST_F(ImuTest, OrientationDisabled)
+{
+  imuMsgs.clear();
+
+  // Start server
+  ServerConfig serverConfig;
+  const auto sdfFile = std::string(PROJECT_SOURCE_PATH) +
+    "/test/worlds/imu_no_orientation.sdf";
+  serverConfig.SetSdfFile(sdfFile);
+
+  Server server(serverConfig);
+  EXPECT_FALSE(server.Running());
+  EXPECT_FALSE(*server.Running(0));
+
+  const std::string sensorName = "imu_sensor";
+
+  auto topic =
+      "world/imu_sensor/model/imu_model/link/link/sensor/imu_sensor/imu";
+
+  // subscribe to imu topic
+  transport::Node node;
+  node.Subscribe(topic, &imuCb);
+
+  // step world and verify imu's orientation is not published
+  // Run server
+  size_t iters200 = 200u;
+  server.Run(true, iters200, false);
+
+  // Check we received messages
+  EXPECT_GT(imuMsgs.size(), 0u);
+  mutex.lock();
+  for (const auto &msg : imuMsgs)
+  {
+    EXPECT_FALSE(msg.has_orientation());
+  }
+  mutex.unlock();
+}