From 705b514b4b02bcea7c501caac49d8783b56d576f Mon Sep 17 00:00:00 2001 From: henrykotze Date: Sat, 25 Nov 2023 20:52:12 +0200 Subject: [PATCH 1/7] Copied Airspeed Sensor PR: #1847 and renamed Signed-off-by: henrykotze --- CMakeLists.txt | 1 + examples/worlds/sensors.sdf | 21 ++ include/gz/sim/components/AirFlowSensor.hh | 46 +++ src/Conversions.cc | 1 + src/SdfEntityCreator.cc | 14 + src/SdfGenerator.cc | 9 + src/systems/CMakeLists.txt | 1 + src/systems/air_flow/AirFlow.cc | 310 ++++++++++++++++++ src/systems/air_flow/AirFlow.hh | 65 ++++ src/systems/air_flow/CMakeLists.txt | 8 + .../scene_broadcaster/SceneBroadcaster.cc | 7 + test/integration/CMakeLists.txt | 1 + test/integration/air_flow_system.cc | 136 ++++++++ test/worlds/air_flow.sdf | 67 ++++ 14 files changed, 687 insertions(+) create mode 100644 include/gz/sim/components/AirFlowSensor.hh create mode 100644 src/systems/air_flow/AirFlow.cc create mode 100644 src/systems/air_flow/AirFlow.hh create mode 100644 src/systems/air_flow/CMakeLists.txt create mode 100644 test/integration/air_flow_system.cc create mode 100644 test/worlds/air_flow.sdf diff --git a/CMakeLists.txt b/CMakeLists.txt index 7185f31695..98d88c42e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -139,6 +139,7 @@ gz_find_package(gz-sensors7 REQUIRED VERSION 7.1 # component order is important COMPONENTS # non-rendering + air_flow air_pressure air_speed altimeter diff --git a/examples/worlds/sensors.sdf b/examples/worlds/sensors.sdf index 9717bb11b3..dd072f9427 100644 --- a/examples/worlds/sensors.sdf +++ b/examples/worlds/sensors.sdf @@ -136,6 +136,27 @@ + + 1 + 10 + true + air_flow + true + + + + 0.2 + 0.1 + + + + + 0.2 + 0.1 + + + + 1 30 diff --git a/include/gz/sim/components/AirFlowSensor.hh b/include/gz/sim/components/AirFlowSensor.hh new file mode 100644 index 0000000000..2f33b42296 --- /dev/null +++ b/include/gz/sim/components/AirFlowSensor.hh @@ -0,0 +1,46 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_SIM_COMPONENTS_AIRFLOW_HH_ +#define GZ_SIM_COMPONENTS_AIRFLOW_HH_ + +#include + +#include +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace components +{ + /// \brief A component type that contains an air flow sensor, + /// sdf::AirFlow, information. + using AirFlowSensor = Component; + GZ_SIM_REGISTER_COMPONENT("gz_sim_components.AirFlowSensor", + AirFlowSensor) +} +} +} +} + +#endif diff --git a/src/Conversions.cc b/src/Conversions.cc index d7d67f3b34..206ff0b7c4 100644 --- a/src/Conversions.cc +++ b/src/Conversions.cc @@ -46,6 +46,7 @@ #include #include +#include #include #include #include diff --git a/src/SdfEntityCreator.cc b/src/SdfEntityCreator.cc index fba9663f3e..b30c8abca0 100644 --- a/src/SdfEntityCreator.cc +++ b/src/SdfEntityCreator.cc @@ -33,6 +33,7 @@ #include "gz/sim/components/components.hh" #else #include "gz/sim/components/Actor.hh" +#include "gz/sim/components/AirFlowSensor.hh" #include "gz/sim/components/AirPressureSensor.hh" #include "gz/sim/components/AirSpeedSensor.hh" #include "gz/sim/components/Altimeter.hh" @@ -994,6 +995,19 @@ Entity SdfEntityCreator::CreateEntities(const sdf::Sensor *_sensor) this->dataPtr->ecm->CreateComponent(sensorEntity, components::WideAngleCamera(*_sensor)); } + else if (_sensor->Type() == sdf::SensorType::AIR_FLOW) + { + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::AirFlowSensor(*_sensor)); + + // create components to be filled by physics + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldPose(math::Pose3d::Zero)); + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldLinearVelocity(math::Vector3d::Zero)); + this->dataPtr->ecm->CreateComponent(sensorEntity, + components::WorldAngularVelocity(math::Vector3d::Zero)); + } else if (_sensor->Type() == sdf::SensorType::AIR_PRESSURE) { this->dataPtr->ecm->CreateComponent(sensorEntity, diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index 05184eed57..c10f4ca0de 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -716,6 +716,15 @@ namespace sdf_generator _elem = contactComp->Data(); return updateSensorNameAndPose(); } + // air flow + auto airFlowComp = + _ecm.Component(_entity); + if (airFlowComp) + { + const sdf::Sensor &sensor = airFlowComp->Data(); + _elem->Copy(sensor.ToElement()); + return updateSensorNameAndPose(); + } // air pressure auto airPressureComp = _ecm.Component(_entity); diff --git a/src/systems/CMakeLists.txt b/src/systems/CMakeLists.txt index c51e78b606..e31be64121 100644 --- a/src/systems/CMakeLists.txt +++ b/src/systems/CMakeLists.txt @@ -97,6 +97,7 @@ endfunction() add_subdirectory(ackermann_steering) add_subdirectory(acoustic_comms) add_subdirectory(advanced_lift_drag) +add_subdirectory(air_flow) add_subdirectory(air_pressure) add_subdirectory(air_speed) add_subdirectory(altimeter) diff --git a/src/systems/air_flow/AirFlow.cc b/src/systems/air_flow/AirFlow.cc new file mode 100644 index 0000000000..f25fa4532e --- /dev/null +++ b/src/systems/air_flow/AirFlow.cc @@ -0,0 +1,310 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "AirFlow.hh" + +#include + +#include +#include +#include +#include + +#include + +#include + +#include + +#include + +#include +#include + +#include "gz/sim/components/AirFlowSensor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/ParentEntity.hh" +#include "gz/sim/components/Pose.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Util.hh" + +using namespace gz; +using namespace sim; +using namespace systems; + +/// \brief Private AirFlow data class. +class gz::sim::systems::AirFlowPrivate +{ + /// \brief A map of air flow entity to its sensor + public: std::unordered_map> entitySensorMap; + + /// \brief gz-sensors sensor factory for creating sensors + public: sensors::SensorFactory sensorFactory; + + /// \brief Keep list of sensors that were created during the previous + /// `PostUpdate`, so that components can be created during the next + /// `PreUpdate`. + public: std::unordered_set newSensors; + + /// True if the rendering component is initialized + public: bool initialized = false; + + public: Entity entity; + + /// \brief Create sensor + /// \param[in] _ecm Immutable reference to ECM. + /// \param[in] _entity Entity of the IMU + /// \param[in] _AirFlow AirFlowSensor component. + /// \param[in] _parent Parent entity component. + public: void AddAirFlow( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::AirFlowSensor *_AirFlow, + const components::ParentEntity *_parent); + + /// \brief Create air flow sensor + /// \param[in] _ecm Immutable reference to ECM. + public: void CreateSensors(const EntityComponentManager &_ecm); + + /// \brief Update air flow sensor data based on physics data + /// \param[in] _ecm Immutable reference to ECM. + public: void UpdateAirFlows(const EntityComponentManager &_ecm); + + /// \brief Remove air flow sensors if their entities have been removed + /// from simulation. + /// \param[in] _ecm Immutable reference to ECM. + public: void RemoveAirFlowEntities(const EntityComponentManager &_ecm); +}; + +////////////////////////////////////////////////// +AirFlow::AirFlow() : + System(), dataPtr(std::make_unique()) +{ +} + +////////////////////////////////////////////////// +AirFlow::~AirFlow() = default; + +////////////////////////////////////////////////// +void AirFlow::PreUpdate(const UpdateInfo &/*_info*/, + EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirFlow::PreUpdate"); + + // Create components + for (auto entity : this->dataPtr->newSensors) + { + auto it = this->dataPtr->entitySensorMap.find(entity); + if (it == this->dataPtr->entitySensorMap.end()) + { + gzerr << "Entity [" << entity + << "] isn't in sensor map, this shouldn't happen." << std::endl; + continue; + } + // Set topic + _ecm.CreateComponent(entity, components::SensorTopic(it->second->Topic())); + } + this->dataPtr->newSensors.clear(); +} + +////////////////////////////////////////////////// +void AirFlow::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) +{ + // Only update and publish if not paused. + GZ_PROFILE("AirFlow::PostUpdate"); + + // \TODO(anyone) Support rewind + if (_info.dt < std::chrono::steady_clock::duration::zero()) + { + gzwarn << "Detected jump back in time [" + << std::chrono::duration_cast(_info.dt).count() + << "s]. System may not work properly." << std::endl; + } + + this->dataPtr->CreateSensors(_ecm); + + if (!_info.paused) + { + // check to see if update is necessary + // we only update if there is at least one sensor that needs data + // and that sensor has subscribers. + // note: gz-sensors does its own throttling. Here the check is mainly + // to avoid doing work in the AirFlowPrivate::UpdateSpeeds function + bool needsUpdate = false; + for (auto &it : this->dataPtr->entitySensorMap) + { + if (it.second->NextDataUpdateTime() <= _info.simTime && + it.second->HasConnections()) + { + needsUpdate = true; + break; + } + } + if (!needsUpdate) + return; + + this->dataPtr->UpdateAirFlows(_ecm); + + for (auto &it : this->dataPtr->entitySensorMap) + { + // Update measurement time + it.second->Update(_info.simTime, false); + } + } + + this->dataPtr->RemoveAirFlowEntities(_ecm); +} + +////////////////////////////////////////////////// +void AirFlowPrivate::AddAirFlow( + const EntityComponentManager &_ecm, + const Entity _entity, + const components::AirFlowSensor *_AirFlow, + const components::ParentEntity *_parent) +{ + this->entity = _entity; + // create sensor + std::string sensorScopedName = + removeParentScope(scopedName(_entity, _ecm, "::", false), "::"); + sdf::Sensor data = _AirFlow->Data(); + data.SetName(sensorScopedName); + // check topic + if (data.Topic().empty()) + { + std::string topic = scopedName(_entity, _ecm) + "/air_flow"; + data.SetTopic(topic); + } + std::unique_ptr sensor = + this->sensorFactory.CreateSensor< + sensors::AirFlowSensor>(data); + if (nullptr == sensor) + { + gzerr << "Failed to create sensor [" << sensorScopedName << "]" + << std::endl; + return; + } + + // set sensor parent + std::string parentName = _ecm.Component( + _parent->Data())->Data(); + sensor->SetParent(parentName); + + // The WorldPose component was just created and so it's empty + // We'll compute the world pose manually here + // set sensor world pose + math::Pose3d sensorWorldPose = worldPose(_entity, _ecm); + sensor->SetPose(sensorWorldPose); + + this->entitySensorMap.insert( + std::make_pair(_entity, std::move(sensor))); + this->newSensors.insert(_entity); +} + +////////////////////////////////////////////////// +void AirFlowPrivate::CreateSensors(const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirFlowPrivate::CreateAirFlowEntities"); + if (!this->initialized) + { + // Create air flow sensors + _ecm.Each( + [&](const Entity &_entity, + const components::AirFlowSensor *_AirFlow, + const components::ParentEntity *_parent)->bool + { + this->AddAirFlow(_ecm, _entity, _AirFlow, _parent); + return true; + }); + this->initialized = true; + } + else + { + // Create air flow sensors + _ecm.EachNew( + [&](const Entity &_entity, + const components::AirFlowSensor *_AirFlow, + const components::ParentEntity *_parent)->bool + { + this->AddAirFlow(_ecm, _entity, _AirFlow, _parent); + return true; + }); + } +} + +////////////////////////////////////////////////// +void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirFlowPrivate::UpdateAirFlows"); + _ecm.Each( + [&](const Entity &_entity, + const components::AirFlowSensor *, + const components::WorldPose *_worldPose)->bool + { + auto it = this->entitySensorMap.find(_entity); + if (it != this->entitySensorMap.end()) + { + const math::Pose3d &worldPose = _worldPose->Data(); + it->second->SetPose(worldPose); + + math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm); + it->second->SetVelocity(sensorRelativeVel); + } + else + { + gzerr << "Failed to update air flow: " << _entity << ". " + << "Entity not found." << std::endl; + } + + return true; + }); +} + +////////////////////////////////////////////////// +void AirFlowPrivate::RemoveAirFlowEntities( + const EntityComponentManager &_ecm) +{ + GZ_PROFILE("AirFlowPrivate::RemoveAirFlowEntities"); + _ecm.EachRemoved( + [&](const Entity &_entity, + const components::AirFlowSensor *)->bool + { + auto sensorId = this->entitySensorMap.find(_entity); + if (sensorId == this->entitySensorMap.end()) + { + gzerr << "Internal error, missing air flow sensor for entity [" + << _entity << "]" << std::endl; + return true; + } + + this->entitySensorMap.erase(sensorId); + + return true; + }); +} + +GZ_ADD_PLUGIN(AirFlow, System, + AirFlow::ISystemPreUpdate, + AirFlow::ISystemPostUpdate +) + +GZ_ADD_PLUGIN_ALIAS(AirFlow, "gz::sim::systems::AirFlow") + +// TODO(CH3): Deprecated, remove on version 8 +GZ_ADD_PLUGIN_ALIAS(AirFlow, "ignition::gazebo::systems::AirFlow") diff --git a/src/systems/air_flow/AirFlow.hh b/src/systems/air_flow/AirFlow.hh new file mode 100644 index 0000000000..62775ba916 --- /dev/null +++ b/src/systems/air_flow/AirFlow.hh @@ -0,0 +1,65 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * +*/ +#ifndef GZ_SIM_SYSTEMS_AIRFLOW_HH_ +#define GZ_SIM_SYSTEMS_AIRFLOW_HH_ + +#include +#include +#include + +namespace gz +{ +namespace sim +{ +// Inline bracket to help doxygen filtering. +inline namespace GZ_SIM_VERSION_NAMESPACE { +namespace systems +{ + // Forward declarations. + class AirFlowPrivate; + + /// \class AirFlow AirFlow.hh gz/sim/systems/AirFlow.hh + /// \brief An air flow sensor that reports vertical position and velocity + /// readings over gz transport + class AirFlow: + public System, + public ISystemPreUpdate, + public ISystemPostUpdate + { + /// \brief Constructor + public: explicit AirFlow(); + + /// \brief Destructor + public: ~AirFlow() override; + + /// Documentation inherited + public: void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + + /// Documentation inherited + public: void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) final; + + /// \brief Private data pointer. + private: std::unique_ptr dataPtr; + }; + } +} +} +} +#endif diff --git a/src/systems/air_flow/CMakeLists.txt b/src/systems/air_flow/CMakeLists.txt new file mode 100644 index 0000000000..4c472bf39a --- /dev/null +++ b/src/systems/air_flow/CMakeLists.txt @@ -0,0 +1,8 @@ +gz_add_system(air-flow + SOURCES + AirFlow.cc + PUBLIC_LINK_LIBS + gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER} + PRIVATE_LINK_LIBS + gz-sensors${GZ_SENSORS_VER}::air_flow +) diff --git a/src/systems/scene_broadcaster/SceneBroadcaster.cc b/src/systems/scene_broadcaster/SceneBroadcaster.cc index b9a4efce52..5ef0d3b6c5 100644 --- a/src/systems/scene_broadcaster/SceneBroadcaster.cc +++ b/src/systems/scene_broadcaster/SceneBroadcaster.cc @@ -32,6 +32,7 @@ #include #include +#include "gz/sim/components/AirFlowSensor.hh" #include "gz/sim/components/AirPressureSensor.hh" #include "gz/sim/components/AirSpeedSensor.hh" #include "gz/sim/components/Altimeter.hh" @@ -907,6 +908,12 @@ void SceneBroadcasterPrivate::SceneGraphAddEntities( { sensorMsg->set_type("altimeter"); } + auto airFlowComp = _manager.Component< + components::AirFlowSensor>(_entity); + if (airFlowComp) + { + sensorMsg->set_type("air_flow"); + } auto airPressureComp = _manager.Component< components::AirPressureSensor>(_entity); if (airPressureComp) diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index fedae18b24..0fdbf0cd70 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -4,6 +4,7 @@ set(tests ackermann_steering_system.cc actor.cc acoustic_comms.cc + air_flow_system.cc air_pressure_system.cc air_speed_system.cc altimeter_system.cc diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc new file mode 100644 index 0000000000..94b8721f1c --- /dev/null +++ b/test/integration/air_flow_system.cc @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2023 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include + +#include + +#include +#include +#include +#include +#include + +#include "gz/sim/components/AirFlowSensor.hh" +#include "gz/sim/components/Name.hh" +#include "gz/sim/components/Sensor.hh" +#include "gz/sim/Server.hh" +#include "test_config.hh" + +#include "../helpers/Relay.hh" +#include "../helpers/EnvTestFixture.hh" + +using namespace gz; +using namespace sim; + +/// \brief Test AirFlowTest system +class AirFlowTest : public InternalFixture<::testing::Test> +{ +}; + +///////////////////////////////////////////////// +// See https://github.com/gazebosim/gz-sim/issues/1175 +TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlow)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "air_flow.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "air_flow_sensor"; + + auto topic = "world/air_flow_sensor/model/air_flow_model/link/link/" + "sensor/air_flow_sensor/air_flow"; + + bool updateChecked{false}; + + // Create a system that checks sensor topic + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, + const components::AirFlowSensor *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + updateChecked = true; + + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // Subscribe to air_flow topic + bool received{false}; + msgs::AirFlowSensor msg; + msg.Clear(); + std::function cb = + [&received, &msg](const msgs::AirFlowSensor &_msg) + { + // Only need one message + if (received) + return; + + msg = _msg; + received = true; + }; + + transport::Node node; + node.Subscribe(topic, cb); + + // Run server + server.Run(true, 100, false); + EXPECT_TRUE(updateChecked); + + // Wait for message to be received + for (int sleep = 0; !received && sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(received); + + // check air pressure + EXPECT_TRUE(msg.has_header()); + EXPECT_TRUE(msg.header().has_stamp()); + EXPECT_EQ(0, msg.header().stamp().sec()); + EXPECT_LT(0, msg.header().stamp().nsec()); + EXPECT_DOUBLE_EQ(0, msg.diff_pressure()); + EXPECT_DOUBLE_EQ(288.14999389648438, msg.temperature()); +} diff --git a/test/worlds/air_flow.sdf b/test/worlds/air_flow.sdf new file mode 100644 index 0000000000..1c2825cf1a --- /dev/null +++ b/test/worlds/air_flow.sdf @@ -0,0 +1,67 @@ + + + + + 0 + + + + + + + + + true + 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 + 10 + true + + + + 0 + 0 + + + + + 0 + 0 + + + + + + + + + From 8f03a50e079cb504b4d581818654f2b9b00116c9 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Sun, 26 Nov 2023 09:01:17 +0200 Subject: [PATCH 2/7] Added Wind Measurement to airflow sensor Signed-off-by: henrykotze --- src/systems/air_flow/AirFlow.cc | 12 ++++++++++++ test/integration/air_flow_system.cc | 6 +++--- 2 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/systems/air_flow/AirFlow.cc b/src/systems/air_flow/AirFlow.cc index f25fa4532e..c9503c1cfd 100644 --- a/src/systems/air_flow/AirFlow.cc +++ b/src/systems/air_flow/AirFlow.cc @@ -40,6 +40,7 @@ #include "gz/sim/components/ParentEntity.hh" #include "gz/sim/components/Pose.hh" #include "gz/sim/components/Sensor.hh" +#include "gz/sim/components/Wind.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Util.hh" @@ -263,8 +264,19 @@ void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm) const math::Pose3d &worldPose = _worldPose->Data(); it->second->SetPose(worldPose); + + // get wind as a component from the _ecm + components::WorldLinearVelocity *windLinearVel = nullptr; + if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + windLinearVel = + _ecm.Component(windEntity); + } + math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm); it->second->SetVelocity(sensorRelativeVel); + + it->second->SetWindVelocity(windLinearVel); } else { diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc index 94b8721f1c..eed8c960f2 100644 --- a/test/integration/air_flow_system.cc +++ b/test/integration/air_flow_system.cc @@ -126,11 +126,11 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlow)) } EXPECT_TRUE(received); - // check air pressure + // check air flow EXPECT_TRUE(msg.has_header()); EXPECT_TRUE(msg.header().has_stamp()); EXPECT_EQ(0, msg.header().stamp().sec()); EXPECT_LT(0, msg.header().stamp().nsec()); - EXPECT_DOUBLE_EQ(0, msg.diff_pressure()); - EXPECT_DOUBLE_EQ(288.14999389648438, msg.temperature()); + EXPECT_DOUBLE_EQ(0, msg.direction()); + EXPECT_DOUBLE_EQ(288.14999389648438, msg.speed()); } From 381297337b8c04ebcdad6bf7f784d3f9b8ed1158 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Mon, 27 Nov 2023 07:41:03 +0200 Subject: [PATCH 3/7] compiling succesfully with Wind Component - Need to add more tests, alongside a windy world environment Signed-off-by: henrykotze --- src/SdfGenerator.cc | 1 + src/systems/air_flow/AirFlow.cc | 33 ++++++++++++++++++----------- test/integration/air_flow_system.cc | 4 ++-- 3 files changed, 24 insertions(+), 14 deletions(-) diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index c10f4ca0de..b1277d2d82 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -26,6 +26,7 @@ #include #include "gz/sim/Util.hh" +#include "gz/sim/components/AirFlowSensor.hh" #include "gz/sim/components/AirPressureSensor.hh" #include "gz/sim/components/AirSpeedSensor.hh" #include "gz/sim/components/Altimeter.hh" diff --git a/src/systems/air_flow/AirFlow.cc b/src/systems/air_flow/AirFlow.cc index c9503c1cfd..f742b17418 100644 --- a/src/systems/air_flow/AirFlow.cc +++ b/src/systems/air_flow/AirFlow.cc @@ -17,7 +17,7 @@ #include "AirFlow.hh" -#include +#include #include #include @@ -41,6 +41,8 @@ #include "gz/sim/components/Pose.hh" #include "gz/sim/components/Sensor.hh" #include "gz/sim/components/Wind.hh" +#include "gz/sim/components/World.hh" +#include "gz/sim/components/LinearVelocity.hh" #include "gz/sim/EntityComponentManager.hh" #include "gz/sim/Util.hh" @@ -85,7 +87,7 @@ class gz::sim::systems::AirFlowPrivate /// \brief Update air flow sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. - public: void UpdateAirFlows(const EntityComponentManager &_ecm); + public: void UpdateAirFlows(const EntityComponentManager &_ecm, gz::math::Vector3d wind); /// \brief Remove air flow sensors if their entities have been removed /// from simulation. @@ -143,6 +145,20 @@ void AirFlow::PostUpdate(const UpdateInfo &_info, if (!_info.paused) { + + + gz::math::Vector3d wind{0,0,0}; + // get wind as a component from the _ecm + if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ + + Entity windEntity = _ecm.EntityByComponents(components::Wind()); + auto windLinearVel = _ecm.Component(windEntity); + + if (windLinearVel != nullptr){ + wind = windLinearVel->Data(); + } + } + // check to see if update is necessary // we only update if there is at least one sensor that needs data // and that sensor has subscribers. @@ -161,7 +177,7 @@ void AirFlow::PostUpdate(const UpdateInfo &_info, if (!needsUpdate) return; - this->dataPtr->UpdateAirFlows(_ecm); + this->dataPtr->UpdateAirFlows(_ecm, wind); for (auto &it : this->dataPtr->entitySensorMap) { @@ -250,7 +266,7 @@ void AirFlowPrivate::CreateSensors(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm) +void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm, math::Vector3d wind) { GZ_PROFILE("AirFlowPrivate::UpdateAirFlows"); _ecm.Each( @@ -265,18 +281,11 @@ void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm) it->second->SetPose(worldPose); - // get wind as a component from the _ecm - components::WorldLinearVelocity *windLinearVel = nullptr; - if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ - Entity windEntity = _ecm.EntityByComponents(components::Wind()); - windLinearVel = - _ecm.Component(windEntity); - } math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm); it->second->SetVelocity(sensorRelativeVel); - it->second->SetWindVelocity(windLinearVel); + it->second->SetWindVelocity(wind); } else { diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc index eed8c960f2..f344240dbd 100644 --- a/test/integration/air_flow_system.cc +++ b/test/integration/air_flow_system.cc @@ -131,6 +131,6 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlow)) EXPECT_TRUE(msg.header().has_stamp()); EXPECT_EQ(0, msg.header().stamp().sec()); EXPECT_LT(0, msg.header().stamp().nsec()); - EXPECT_DOUBLE_EQ(0, msg.direction()); - EXPECT_DOUBLE_EQ(288.14999389648438, msg.speed()); + EXPECT_DOUBLE_EQ(0, msg.xy_direction()); + EXPECT_DOUBLE_EQ(0, msg.xy_speed()); } From d36f0ab7d4816b5bed358e22239ba4799aac0f36 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Mon, 27 Nov 2023 19:41:36 +0200 Subject: [PATCH 4/7] Integration tests are running succesfully Signed-off-by: henrykotze --- test/integration/air_flow_system.cc | 81 +++++++++++++++++++++++++ test/worlds/air_flow_windy.sdf | 93 +++++++++++++++++++++++++++++ 2 files changed, 174 insertions(+) create mode 100644 test/worlds/air_flow_windy.sdf diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc index f344240dbd..9792e02df4 100644 --- a/test/integration/air_flow_system.cc +++ b/test/integration/air_flow_system.cc @@ -134,3 +134,84 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlow)) EXPECT_DOUBLE_EQ(0, msg.xy_direction()); EXPECT_DOUBLE_EQ(0, msg.xy_speed()); } + +TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowWindy)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "air_flow_windy.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "air_flow_sensor"; + + auto topic = "world/air_flow_sensor/model/air_flow_model/link/link/" + "sensor/air_flow_sensor/air_flow"; + + bool updateChecked{false}; + + // Create a system that checks sensor topic + test::Relay testSystem; + testSystem.OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, + const components::AirFlowSensor *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + updateChecked = true; + + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + + // Subscribe to air_flow topic + bool received{false}; + msgs::AirFlowSensor msg; + msg.Clear(); + std::function cb = + [&received, &msg](const msgs::AirFlowSensor &_msg) + { + + msg = _msg; + received = true; + }; + + transport::Node node; + node.Subscribe(topic, cb); + + // Run server + server.Run(true, 10, false); + EXPECT_TRUE(updateChecked); + + EXPECT_TRUE(received); + + // check air flow + EXPECT_TRUE(msg.has_header()); + EXPECT_TRUE(msg.header().has_stamp()); + EXPECT_NEAR(5.0, msg.xy_speed(), 1e-3); + EXPECT_NEAR(-M_PI_2, msg.xy_direction(), 1e-2); +} diff --git a/test/worlds/air_flow_windy.sdf b/test/worlds/air_flow_windy.sdf new file mode 100644 index 0000000000..04efee5f73 --- /dev/null +++ b/test/worlds/air_flow_windy.sdf @@ -0,0 +1,93 @@ + + + + + 0 + + + + + + + + + 1 + + + 0.0 + + 0.0 + 0.0 + + + + 0.0 + + 0.0 + 0.0 + + + + + + + 0 5.0 0 + + + + true + 4 0 3.0 0 0.0 0 + + 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 + 10 + true + + + + 0 + 0 + + + + + 0 + 0 + + + + + + + + + From 62a58c6a3d75c6fdf903cba38455b43029a34533 Mon Sep 17 00:00:00 2001 From: henrykotze Date: Tue, 28 Nov 2023 13:58:28 +0200 Subject: [PATCH 5/7] Wind Sensor Integration testing working Signed-off-by: henrykotze --- src/systems/air_flow/AirFlow.cc | 13 ++- test/integration/air_flow_system.cc | 140 +++++++++++++++++++++++++++- test/worlds/air_flow_windy.sdf | 32 ++++--- 3 files changed, 167 insertions(+), 18 deletions(-) diff --git a/src/systems/air_flow/AirFlow.cc b/src/systems/air_flow/AirFlow.cc index f742b17418..6f516ee4c7 100644 --- a/src/systems/air_flow/AirFlow.cc +++ b/src/systems/air_flow/AirFlow.cc @@ -156,6 +156,8 @@ void AirFlow::PostUpdate(const UpdateInfo &_info, if (windLinearVel != nullptr){ wind = windLinearVel->Data(); + }else { + wind = gz::math::Vector3d::Zero; } } @@ -280,12 +282,17 @@ void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm, math::Ve const math::Pose3d &worldPose = _worldPose->Data(); it->second->SetPose(worldPose); - - math::Vector3d sensorRelativeVel = relativeVel(_entity, _ecm); it->second->SetVelocity(sensorRelativeVel); - it->second->SetWindVelocity(wind); + if(wind.IsFinite()){ + it->second->SetWindVelocity(wind); + } else + { + gzwarn << "Wind velocity is ignored. Contains NaN"; + it->second->SetWindVelocity(gz::math::Vector3d::Zero); + } + } else { diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc index 9792e02df4..d2e351d087 100644 --- a/test/integration/air_flow_system.cc +++ b/test/integration/air_flow_system.cc @@ -25,14 +25,19 @@ #include #include +#include "gz/sim/Link.hh" +#include "gz/sim/Model.hh" #include "gz/sim/components/AirFlowSensor.hh" #include "gz/sim/components/Name.hh" #include "gz/sim/components/Sensor.hh" +#include "gz/sim/World.hh" #include "gz/sim/Server.hh" #include "test_config.hh" +#include "gz/sim/TestFixture.hh" #include "../helpers/Relay.hh" #include "../helpers/EnvTestFixture.hh" +#include "test_config.hh" using namespace gz; using namespace sim; @@ -131,10 +136,13 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlow)) EXPECT_TRUE(msg.header().has_stamp()); EXPECT_EQ(0, msg.header().stamp().sec()); EXPECT_LT(0, msg.header().stamp().nsec()); - EXPECT_DOUBLE_EQ(0, msg.xy_direction()); + EXPECT_DOUBLE_EQ(-M_PI_2, msg.xy_direction()); EXPECT_DOUBLE_EQ(0, msg.xy_speed()); } +///////////////////////////////////////////////// +// The test checks if the sensor is capable of measuring +// the TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowWindy)) { // Start server @@ -204,14 +212,138 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowWindy)) node.Subscribe(topic, cb); // Run server - server.Run(true, 10, false); + server.Run(true, 100, false); EXPECT_TRUE(updateChecked); + + // Wait for message to be received + for (int sleep = 0; sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(received); + + // check air flow + EXPECT_TRUE(msg.has_header()); + EXPECT_TRUE(msg.header().has_stamp()); + EXPECT_EQ(5.0, msg.xy_speed()); + EXPECT_NEAR(0.0, msg.xy_direction(), 1e-3); +} + +///////////////////////////////////////////////// +// The test checks if the sensor is moving at the same speed as the +// wind, that it measure no airflow +TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveWindy)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "air_flow_windy.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "air_flow_sensor"; + + auto topic = "world/air_flow_sensor/model/air_flow_model/link/link/" + "sensor/air_flow_sensor/air_flow"; + + bool updateChecked{false}; + + // Create a system that checks sensor topic + TestFixture testSystem(serverConfig); + + Link body; + Model model; + + + testSystem.OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + auto modelEntity = world.ModelByName(_ecm, "air_flow_model"); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, "link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + body.SetLinearVelocity(_ecm, gz::math::Vector3d(1,5,0)); + + + }).OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + + _ecm.Each( + [&](const Entity &_entity, + const components::AirFlowSensor *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + updateChecked = true; + + return true; + }); + }).Finalize(); + + // Subscribe to air_flow topic + bool received{false}; + msgs::AirFlowSensor msg; + msg.Clear(); + std::function cb = + [&received, &msg](const msgs::AirFlowSensor &_msg) + { + + msg = _msg; + received = true; + }; + + transport::Node node; + node.Subscribe(topic, cb); + + + testSystem.Server()->Run(true, 10, false); + // Run server + // server.Run(true, 10, false); + EXPECT_TRUE(updateChecked); + + + // Wait for message to be received + for (int sleep = 0; sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + EXPECT_TRUE(received); // check air flow EXPECT_TRUE(msg.has_header()); EXPECT_TRUE(msg.header().has_stamp()); - EXPECT_NEAR(5.0, msg.xy_speed(), 1e-3); - EXPECT_NEAR(-M_PI_2, msg.xy_direction(), 1e-2); + EXPECT_NEAR(10.04987562, msg.xy_speed(), 1e-3); + EXPECT_NEAR(-0.0996686524911, msg.xy_direction(), 1e-2); } diff --git a/test/worlds/air_flow_windy.sdf b/test/worlds/air_flow_windy.sdf index 04efee5f73..566e6cd083 100644 --- a/test/worlds/air_flow_windy.sdf +++ b/test/worlds/air_flow_windy.sdf @@ -1,10 +1,15 @@ - + + + 0.001 + 0 + 0 0 0 + @@ -15,33 +20,38 @@ + + + - 0 5.0 0 + 0 -5.0 0 - true + 4 0 3.0 0 0.0 0 0.05 0.05 0.05 0 0 0 From 21432b2bf3e30bd160a1744a1b58e8d7ba6d7dfb Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 29 Nov 2023 12:39:56 +0200 Subject: [PATCH 6/7] codecheck passed updates Signed-off-by: henrykotze --- src/systems/air_flow/AirFlow.cc | 16 +++++++++------- test/integration/air_flow_system.cc | 10 ++++------ 2 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/systems/air_flow/AirFlow.cc b/src/systems/air_flow/AirFlow.cc index 6f516ee4c7..084cbe239d 100644 --- a/src/systems/air_flow/AirFlow.cc +++ b/src/systems/air_flow/AirFlow.cc @@ -87,7 +87,9 @@ class gz::sim::systems::AirFlowPrivate /// \brief Update air flow sensor data based on physics data /// \param[in] _ecm Immutable reference to ECM. - public: void UpdateAirFlows(const EntityComponentManager &_ecm, gz::math::Vector3d wind); + /// \param[in] wind The wind conditions in the environment + public: void UpdateAirFlows(const EntityComponentManager &_ecm, + gz::math::Vector3d wind); /// \brief Remove air flow sensors if their entities have been removed /// from simulation. @@ -146,13 +148,13 @@ void AirFlow::PostUpdate(const UpdateInfo &_info, if (!_info.paused) { - - gz::math::Vector3d wind{0,0,0}; + gz::math::Vector3d wind{0, 0, 0}; // get wind as a component from the _ecm if(_ecm.EntityByComponents(components::Wind()) != kNullEntity){ Entity windEntity = _ecm.EntityByComponents(components::Wind()); - auto windLinearVel = _ecm.Component(windEntity); + auto windLinearVel = _ecm.Component + (windEntity); if (windLinearVel != nullptr){ wind = windLinearVel->Data(); @@ -268,7 +270,8 @@ void AirFlowPrivate::CreateSensors(const EntityComponentManager &_ecm) } ////////////////////////////////////////////////// -void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm, math::Vector3d wind) +void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm, + math::Vector3d wind) { GZ_PROFILE("AirFlowPrivate::UpdateAirFlows"); _ecm.Each( @@ -287,8 +290,7 @@ void AirFlowPrivate::UpdateAirFlows(const EntityComponentManager &_ecm, math::Ve if(wind.IsFinite()){ it->second->SetWindVelocity(wind); - } else - { + } else { gzwarn << "Wind velocity is ignored. Contains NaN"; it->second->SetWindVelocity(gz::math::Vector3d::Zero); } diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc index d2e351d087..1e0a1fd878 100644 --- a/test/integration/air_flow_system.cc +++ b/test/integration/air_flow_system.cc @@ -37,7 +37,6 @@ #include "../helpers/Relay.hh" #include "../helpers/EnvTestFixture.hh" -#include "test_config.hh" using namespace gz; using namespace sim; @@ -142,7 +141,7 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlow)) ///////////////////////////////////////////////// // The test checks if the sensor is capable of measuring -// the +// the wind conditions when it is stationary TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowWindy)) { // Start server @@ -232,8 +231,8 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowWindy)) } ///////////////////////////////////////////////// -// The test checks if the sensor is moving at the same speed as the -// wind, that it measure no airflow +// The test checks that the sensor measures the correct relative airflow +// when it is moving within windy condition. TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveWindy)) { // Start server @@ -277,9 +276,8 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveWindy)) body = Link(bodyEntity); body.EnableVelocityChecks(_ecm); - body.SetLinearVelocity(_ecm, gz::math::Vector3d(1,5,0)); + body.SetLinearVelocity(_ecm, gz::math::Vector3d(1, 5, 0)); - }).OnPostUpdate([&](const UpdateInfo &_info, const EntityComponentManager &_ecm) { From 55425a756b7f00e3bb15727118077ebc490bdc0b Mon Sep 17 00:00:00 2001 From: henrykotze Date: Wed, 29 Nov 2023 12:51:23 +0200 Subject: [PATCH 7/7] Test no z axis influence measurement Signed-off-by: henrykotze --- test/integration/air_flow_system.cc | 116 ++++++++++++++++++++++++++++ 1 file changed, 116 insertions(+) diff --git a/test/integration/air_flow_system.cc b/test/integration/air_flow_system.cc index 1e0a1fd878..21b648c3fc 100644 --- a/test/integration/air_flow_system.cc +++ b/test/integration/air_flow_system.cc @@ -345,3 +345,119 @@ TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveWindy)) EXPECT_NEAR(10.04987562, msg.xy_speed(), 1e-3); EXPECT_NEAR(-0.0996686524911, msg.xy_direction(), 1e-2); } + +///////////////////////////////////////////////// +// The test checks that the sensor does not include airflow in its z-axis +// in its measurement +TEST_F(AirFlowTest, GZ_UTILS_TEST_DISABLED_ON_WIN32(AirFlowMoveZAxisWindy)) +{ + // Start server + ServerConfig serverConfig; + const auto sdfFile = gz::common::joinPaths(std::string(PROJECT_SOURCE_PATH), + "test", "worlds", "air_flow_windy.sdf"); + serverConfig.SetSdfFile(sdfFile); + + Server server(serverConfig); + EXPECT_FALSE(server.Running()); + EXPECT_FALSE(*server.Running(0)); + + const std::string sensorName = "air_flow_sensor"; + + auto topic = "world/air_flow_sensor/model/air_flow_model/link/link/" + "sensor/air_flow_sensor/air_flow"; + + bool updateChecked{false}; + + // Create a system that checks sensor topic + TestFixture testSystem(serverConfig); + + Link body; + Model model; + + + testSystem.OnConfigure( + [&](const Entity &_worldEntity, + const std::shared_ptr &/*_sdf*/, + EntityComponentManager &_ecm, + EventManager &/*eventMgr*/) + { + World world(_worldEntity); + auto modelEntity = world.ModelByName(_ecm, "air_flow_model"); + EXPECT_NE(modelEntity, kNullEntity); + model = Model(modelEntity); + + auto bodyEntity = model.LinkByName(_ecm, "link"); + EXPECT_NE(bodyEntity, kNullEntity); + + body = Link(bodyEntity); + body.EnableVelocityChecks(_ecm); + + body.SetLinearVelocity(_ecm, gz::math::Vector3d(0, 0, -1)); + + }).OnPostUpdate([&](const UpdateInfo &_info, + const EntityComponentManager &_ecm) + { + + _ecm.Each( + [&](const Entity &_entity, + const components::AirFlowSensor *, + const components::Name *_name) -> bool + { + EXPECT_EQ(_name->Data(), sensorName); + + auto sensorComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, sensorComp); + + if (_info.iterations == 1) + return true; + + // This component is created on the 2nd PreUpdate + auto topicComp = _ecm.Component(_entity); + EXPECT_NE(nullptr, topicComp); + if (topicComp) + { + EXPECT_EQ(topic, topicComp->Data()); + } + + updateChecked = true; + + return true; + }); + }).Finalize(); + + // Subscribe to air_flow topic + bool received{false}; + msgs::AirFlowSensor msg; + msg.Clear(); + std::function cb = + [&received, &msg](const msgs::AirFlowSensor &_msg) + { + + msg = _msg; + received = true; + }; + + transport::Node node; + node.Subscribe(topic, cb); + + + testSystem.Server()->Run(true, 10, false); + // Run server + // server.Run(true, 10, false); + EXPECT_TRUE(updateChecked); + + + // Wait for message to be received + for (int sleep = 0; sleep < 30; ++sleep) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + EXPECT_TRUE(received); + + // check air flow + EXPECT_TRUE(msg.has_header()); + EXPECT_TRUE(msg.header().has_stamp()); + EXPECT_NEAR(5.0, msg.xy_speed(), 1e-3); + EXPECT_NEAR(0.0, msg.xy_direction(), 1e-2); +}