From 96f50bc13132d2eb732e6fe455d7e3b05526f56b Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Thu, 1 Jul 2021 16:54:11 -0700 Subject: [PATCH 1/4] parse imu orientation param Signed-off-by: Ian Chen --- src/systems/imu/Imu.cc | 23 ++++++++++++++++++- src/systems/imu/Imu.hh | 9 +++++++- test/integration/imu_system.cc | 40 ++++++++++++++++++++++++++++++++++ 3 files changed, 70 insertions(+), 2 deletions(-) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 6a645a13ec..09484eec56 100644 --- a/src/systems/imu/Imu.cc +++ b/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()) ////////////////////////////////////////////////// Imu::~Imu() = default; +////////////////////////////////////////////////// +void Imu::Configure(const Entity & /*_entity*/, + const std::shared_ptr &_sdf, + gazebo::EntityComponentManager & /*_ecm*/, + gazebo::EventManager & /*_eventMgr*/) +{ + if (_sdf->HasElement("output_orientation")) + { + this->dataPtr->outputOrientation = + _sdf->Get("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") diff --git a/src/systems/imu/Imu.hh b/src/systems/imu/Imu.hh index 74d5d87750..76cd9bfb52 100644 --- a/src/systems/imu/Imu.hh +++ b/src/systems/imu/Imu.hh @@ -40,7 +40,8 @@ namespace systems class Imu: public System, public ISystemPreUpdate, - public ISystemPostUpdate + public ISystemPostUpdate, + public ISystemConfigure { /// \brief Constructor public: explicit Imu(); @@ -48,6 +49,12 @@ namespace systems /// \brief Destructor public: ~Imu() override; + /// Documentation inherited + public: void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + gazebo::EventManager &_eventMgr) final; + /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index f7ae7b65c2..4d7bcf2f64 100644 --- a/test/integration/imu_system.cc +++ b/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(); +} From 48e171f6e798d4decab97f6eb2129601e45d90db Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 2 Aug 2021 17:02:04 -0700 Subject: [PATCH 2/4] use enable_orientation sdf elem Signed-off-by: Ian Chen --- src/systems/imu/Imu.cc | 28 +++++++--------------------- src/systems/imu/Imu.hh | 9 +-------- 2 files changed, 8 insertions(+), 29 deletions(-) diff --git a/src/systems/imu/Imu.cc b/src/systems/imu/Imu.cc index 09484eec56..1e5f22f443 100644 --- a/src/systems/imu/Imu.cc +++ b/src/systems/imu/Imu.cc @@ -58,10 +58,6 @@ 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 @@ -86,19 +82,6 @@ Imu::Imu() : System(), dataPtr(std::make_unique()) ////////////////////////////////////////////////// Imu::~Imu() = default; -////////////////////////////////////////////////// -void Imu::Configure(const Entity & /*_entity*/, - const std::shared_ptr &_sdf, - gazebo::EntityComponentManager & /*_ecm*/, - gazebo::EventManager & /*_eventMgr*/) -{ - if (_sdf->HasElement("output_orientation")) - { - this->dataPtr->outputOrientation = - _sdf->Get("output_orientation"); - } -} - ////////////////////////////////////////////////// void Imu::PreUpdate(const UpdateInfo &/*_info*/, EntityComponentManager &_ecm) @@ -203,8 +186,12 @@ void ImuPrivate::CreateImuEntities(EntityComponentManager &_ecm) // Set topic _ecm.CreateComponent(_entity, components::SensorTopic(sensor->Topic())); - // Set whether to output orientation - sensor->SetOrientationEnabled(this->outputOrientation); + // Set whether orientation is enabled + if (data.ImuSensor()) + { + sensor->SetOrientationEnabled( + data.ImuSensor()->OrientationEnabled()); + } this->entitySensorMap.insert( std::make_pair(_entity, std::move(sensor))); @@ -274,8 +261,7 @@ void ImuPrivate::RemoveImuEntities( IGNITION_ADD_PLUGIN(Imu, System, Imu::ISystemPreUpdate, - Imu::ISystemPostUpdate, - Imu::ISystemConfigure + Imu::ISystemPostUpdate ) IGNITION_ADD_PLUGIN_ALIAS(Imu, "ignition::gazebo::systems::Imu") diff --git a/src/systems/imu/Imu.hh b/src/systems/imu/Imu.hh index 76cd9bfb52..74d5d87750 100644 --- a/src/systems/imu/Imu.hh +++ b/src/systems/imu/Imu.hh @@ -40,8 +40,7 @@ namespace systems class Imu: public System, public ISystemPreUpdate, - public ISystemPostUpdate, - public ISystemConfigure + public ISystemPostUpdate { /// \brief Constructor public: explicit Imu(); @@ -49,12 +48,6 @@ namespace systems /// \brief Destructor public: ~Imu() override; - /// Documentation inherited - public: void Configure(const Entity &_entity, - const std::shared_ptr &_sdf, - EntityComponentManager &_ecm, - gazebo::EventManager &_eventMgr) final; - /// Documentation inherited public: void PreUpdate(const UpdateInfo &_info, EntityComponentManager &_ecm) final; From 95d485435e52e268a7494a39fe6a03f76c3555c5 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 2 Aug 2021 17:12:03 -0700 Subject: [PATCH 3/4] use joinPaths Signed-off-by: Ian Chen --- test/integration/imu_system.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/test/integration/imu_system.cc b/test/integration/imu_system.cc index 4d7bcf2f64..a774bb19eb 100644 --- a/test/integration/imu_system.cc +++ b/test/integration/imu_system.cc @@ -221,8 +221,8 @@ TEST_F(ImuTest, OrientationDisabled) // Start server ServerConfig serverConfig; - const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/imu_no_orientation.sdf"; + const auto sdfFile = common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "imu_no_orientation.sdf"); serverConfig.SetSdfFile(sdfFile); Server server(serverConfig); From fc22dad182fe0830d0c3b37b686495d23fdafff2 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Wed, 15 Sep 2021 15:57:07 -0700 Subject: [PATCH 4/4] add test world file Signed-off-by: Ian Chen --- test/worlds/imu_no_orientation.sdf | 83 ++++++++++++++++++++++++++++++ 1 file changed, 83 insertions(+) create mode 100644 test/worlds/imu_no_orientation.sdf 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 + + + + + + +