From 705b514b4b02bcea7c501caac49d8783b56d576f Mon Sep 17 00:00:00 2001 From: henrykotze Date: Sat, 25 Nov 2023 20:52:12 +0200 Subject: [PATCH] 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 + + + + + + + + +