Parse new param for enabling / disabling IMU orientation output (#899)

* parse imu orientation param Signed-off-by: Ian Chen <ichen@osrfoundation.org> * use enable_orientation sdf elem Signed-off-by: Ian Chen <ichen@osrfoundation.org> * use joinPaths Signed-off-by: Ian Chen <ichen@osrfoundation.org> * add test world file Signed-off-by: Ian Chen <ichen@osrfoundation.org> Co-authored-by: Louise Poubel <louise@openrobotics.org> Co-authored-by: Nate Koenig <nate@openrobotics.org>
gazebosim · Sep 30, 2021 · 4f3dd67 · 4f3dd67
1 parent 3fbddd1
commit 4f3dd67
Show file tree

Hide file tree

Showing 3 changed files with 130 additions and 0 deletions.
diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc
@@ -186,6 +186,13 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm)
         // Set topic
         _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic()));
 
+        // Set whether orientation is enabled
+        if (data.ImuSensor())
+        {
+          sensor->SetOrientationEnabled(
+              data.ImuSensor()->OrientationEnabled());
+        }
+
         this->entitySensorMap.insert(
             std::make_pair(_entity, std::move(sensor)));
 

diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc
@@ -206,3 +206,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 = common::joinPaths(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();
+}
diff --git a/test/worlds/imu_no_orientation.sdf b/test/worlds/imu_no_orientation.sdf
@@ -0,0 +1,83 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <world name="imu_sensor">
+    <gravity>0 0 -5</gravity>
+    <physics name="1ms" type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1.0</real_time_factor>
+    </physics>
+    <plugin
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-imu-system"
+      name="ignition::gazebo::systems::Imu">
+    </plugin>
+
+    <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>
+
+    <model name="imu_model">
+      <pose>4 0 3.0 0 0.0 3.14</pose>
+      <link name="link">
+        <pose>0.05 0.05 0.05 0 0 0</pose>
+        <inertial>
+          <mass>0.1</mass>
+          <inertia>
+            <ixx>0.000166667</ixx>
+            <iyy>0.000166667</iyy>
+            <izz>0.000166667</izz>
+          </inertia>
+        </inertial>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.1 0.1 0.1</size>
+            </box>
+          </geometry>
+        </visual>
+        <sensor name="imu_sensor" type="imu">
+          <always_on>1</always_on>
+          <update_rate>30</update_rate>
+          <visualize>true</visualize>
+          <imu>
+            <enable_orientation>false</enable_orientation>
+          </imu>
+        </sensor>
+      </link>
+    </model>
+
+  </world>
+</sdf>