diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 6a645a13ec..1e5f22f443 100644 --- 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 index 68147f4e30..b92c34fd68 100644 --- 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 new file mode 100644 index 0000000000..22e17f3663 --- /dev/null +++ b/test/worlds/imu_no_orientation.sdf @@ -0,0 +1,83 @@ + + + + 0 0 -5 + + 0.001 + 1.0 + + + + + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 4 0 3.0 0 0.0 3.14 + + 0.05 0.05 0.05 0 0 0 + + 0.1 + + 0.000166667 + 0.000166667 + 0.000166667 + + + + + + 0.1 0.1 0.1 + + + + + + + 0.1 0.1 0.1 + + + + + 1 + 30 + true + + false + + + + + + +