From 7ab9b0b17471403fce37af87c288d027085407c9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 23 Dec 2022 15:05:58 +0100 Subject: [PATCH 1/9] Added airspeed sensor MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/gz/sensors/AirSpeedSensor.hh | 96 +++++++++++ include/gz/sensors/SensorTypes.hh | 4 + src/AirSpeedSensor.cc | 243 +++++++++++++++++++++++++++ src/CMakeLists.txt | 5 +- test/integration/CMakeLists.txt | 2 +- 5 files changed, 348 insertions(+), 2 deletions(-) create mode 100644 include/gz/sensors/AirSpeedSensor.hh create mode 100644 src/AirSpeedSensor.cc diff --git a/include/gz/sensors/AirSpeedSensor.hh b/include/gz/sensors/AirSpeedSensor.hh new file mode 100644 index 00000000..d0afc634 --- /dev/null +++ b/include/gz/sensors/AirSpeedSensor.hh @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2019 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_SENSORS_AIRSPEEDSENSOR_HH_ +#define GZ_SENSORS_AIRSPEEDSENSOR_HH_ + +#include + +#include + +#include + +#include +#include + +#include "gz/sensors/Sensor.hh" + +namespace gz +{ + namespace sensors + { + // Inline bracket to help doxygen filtering. + inline namespace GZ_SENSORS_VERSION_NAMESPACE { + // + /// \brief forward declarations + class AirSpeedSensorPrivate; + + /// \brief AirSensor Sensor Class + /// + /// A sensor that reports air pressure readings. + class AirSpeedSensor : + public Sensor + { + /// \brief constructor + public: AirSpeedSensor(); + + /// \brief destructor + public: virtual ~AirSpeedSensor(); + + /// \brief Load the sensor based on data from an sdf::Sensor object. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(const sdf::Sensor &_sdf) override; + + /// \brief Load the sensor with SDF parameters. + /// \param[in] _sdf SDF Sensor parameters. + /// \return true if loading was successful + public: virtual bool Load(sdf::ElementPtr _sdf) override; + + /// \brief Initialize values in the sensor + /// \return True on success + public: virtual bool Init() override; + + /// \brief Get the current velocity. + /// \return Current velocity of the sensor. + public: gz::math::Vector3d Velocity() const; + + /// \brief Update the velocity of the sensor + public: void SetVelocity(const gz::math::Vector3d &_vel); + + using Sensor::Update; + + /// \brief Update the sensor and generate data + /// \param[in] _now The current time + /// \return true if the update was successfull + public: virtual bool Update( + const std::chrono::steady_clock::duration &_now) override; + + /// \brief Check if there are any subscribers + /// \return True if there are subscribers, false otherwise + public: virtual bool HasConnections() const override; + + GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING + /// \brief Data pointer for private data + /// \internal + private: std::unique_ptr dataPtr; + GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING + }; + } + } +} + +#endif diff --git a/include/gz/sensors/SensorTypes.hh b/include/gz/sensors/SensorTypes.hh index b8d203df..81607c9b 100644 --- a/include/gz/sensors/SensorTypes.hh +++ b/include/gz/sensors/SensorTypes.hh @@ -205,6 +205,10 @@ namespace gz /// \sa NavSat NAVSAT_VERTICAL_VELOCITY_NOISE = 24, + /// \brief Air speed noise streams for the air speed sensor + /// \sa AirSpeedSensor + AIR_SPEED_NOISE_PASCALS = 25, + /// \internal /// \brief Indicator used to create an iterator over the enum. Do not /// use this. diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc new file mode 100644 index 00000000..4438c248 --- /dev/null +++ b/src/AirSpeedSensor.cc @@ -0,0 +1,243 @@ +/* + * Copyright (C) 2019 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. + * +*/ + +#if defined(_MSC_VER) + #pragma warning(push) + #pragma warning(disable: 4005) + #pragma warning(disable: 4251) +#endif +#include +#if defined(_MSC_VER) + #pragma warning(pop) +#endif +#include + +#include +#include + +#include "gz/sensors/GaussianNoiseModel.hh" +#include "gz/sensors/Noise.hh" +#include "gz/sensors/SensorTypes.hh" +#include "gz/sensors/SensorFactory.hh" +#include "gz/sensors/AirSpeedSensor.hh" + +using namespace gz; +using namespace sensors; + +static constexpr double kPressureOneAtmospherePascals = 101325.0; +// static constexpr double kSeaLevelTempKelvin = 288.15; + +static constexpr auto DEFAULT_HOME_ALT_AMSL = 488.0f; // altitude AMSL at Irchel Park, Zurich, Switzerland [m] + +// international standard atmosphere (troposphere model - valid up to 11km) see [1] +static constexpr auto TEMPERATURE_MSL = 288.15f; // temperature at MSL [K] (15 [C]) +static constexpr auto PRESSURE_MSL = 101325.0f; // pressure at MSL [Pa] +static constexpr auto LAPSE_RATE = 0.0065f; // reduction in temperature with altitude for troposphere [K/m] +static constexpr auto AIR_DENSITY_MSL = 1.225f; // air density at MSL [kg/m^3] + +/// \brief Private data for AirSpeedSensor +class gz::sensors::AirSpeedSensorPrivate +{ + /// \brief node to create publisher + public: transport::Node node; + + /// \brief publisher to publish air pressure messages. + public: transport::Node::Publisher pub; + + /// \brief true if Load() has been called and was successful + public: bool initialized = false; + + /// \brief Pressure in pascals. + public: double pressure = 0.0; + + /// \brief Pose of the sensor + public: gz::math::Vector3d vel; + + /// \brief Noise added to sensor data + public: std::map noises; +}; + +////////////////////////////////////////////////// +AirSpeedSensor::AirSpeedSensor() + : dataPtr(new AirSpeedSensorPrivate()) +{ +} + +////////////////////////////////////////////////// +AirSpeedSensor::~AirSpeedSensor() +{ +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Init() +{ + return this->Sensor::Init(); +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) +{ + if (!Sensor::Load(_sdf)) + return false; + + if (_sdf.Type() != sdf::SensorType::AIR_SPEED) + { + gzerr << "Attempting to a load an AirSpeed sensor, but received " + << "a " << _sdf.TypeStr() << std::endl; + return false; + } + + if (_sdf.AirSpeedSensor() == nullptr) + { + gzerr << "Attempting to a load an AirSpeed sensor, but received " + << "a null sensor." << std::endl; + return false; + } + + if (this->Topic().empty()) + this->SetTopic("/air_speed"); + + this->dataPtr->pub = + this->dataPtr->node.Advertise( + this->Topic()); + + if (!this->dataPtr->pub) + { + gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n"; + return false; + } + + gzdbg << "Air speed for [" << this->Name() << "] advertised on [" + << this->Topic() << "]" << std::endl; + + // Load the noise parameters + if (_sdf.AirSpeedSensor()->SpeedNoise().Type() != sdf::NoiseType::NONE) + { + this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS] = + NoiseFactory::NewNoiseModel(_sdf.AirSpeedSensor()->SpeedNoise()); + } + + this->dataPtr->initialized = true; + return true; +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Load(sdf::ElementPtr _sdf) +{ + sdf::Sensor sdfSensor; + sdfSensor.Load(_sdf); + return this->Load(sdfSensor); +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::Update( + const std::chrono::steady_clock::duration &_now) +{ + GZ_PROFILE("AirSpeedSensor::Update"); + if (!this->dataPtr->initialized) + { + gzerr << "Not initialized, update ignored.\n"; + return false; + } + + msgs::FluidPressure msg; + *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); + auto frame = msg.mutable_header()->add_data(); + frame->set_key("frame_id"); + frame->add_value(this->FrameId()); + + // // This block of code comes from RotorS: + // // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp + // { + // // Get the current height. + // double height = this->dataPtr->referenceAltitude + this->Pose().Pos().Z(); + // + // // Compute the geopotential height. + // double geoHeight = kEarthRadiusMeters * height / + // (kEarthRadiusMeters + height); + // + // // Compute the temperature at the current altitude in Kelvin. + // double tempAtHeight = + // kSeaLevelTempKelvin - kTempLapseKelvinPerMeter * geoHeight; + // + // // Compute the current air pressure. + // this->dataPtr->pressure = + // kPressureOneAtmospherePascals * exp(kAirConstantDimensionless * + // log(kSeaLevelTempKelvin / tempAtHeight)); + // } + + const float alt_rel = static_cast(this->Pose().Pos().Z()); // Z-component from ENU + const float alt_amsl = (float)DEFAULT_HOME_ALT_AMSL + alt_rel; + const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; + const float density_ratio = powf(TEMPERATURE_MSL / temperature_local , 4.256f); + const float air_density = AIR_DENSITY_MSL / density_ratio; + std::cerr << "air_density " << air_density << '\n'; + // // calculate differential pressure + noise in hPa + + gz::math::Vector3d wind_vel_{0, 0, 0}; + gz::math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); + gz::math::Vector3d air_vel_in_body_ = this->dataPtr->vel - veh_q_world_to_body.RotateVectorReverse(wind_vel_); + double diff_pressure = gz::math::sgn(air_vel_in_body_.X()) * 0.005f * air_density * air_vel_in_body_.X() * air_vel_in_body_.X(); + std::cerr << "diff_pressure " << diff_pressure << '\n'; + std::cerr << "this->dataPtr->vel " << this->dataPtr->vel << '\n'; + std::cerr << "veh_q_world_to_body " << veh_q_world_to_body << '\n'; + + // Apply pressure noise + if (this->dataPtr->noises.find(AIR_SPEED_NOISE_PASCALS) != + this->dataPtr->noises.end()) + { + diff_pressure = + this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply( + diff_pressure); + + if (this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Type() == + NoiseType::GAUSSIAN) + { + GaussianNoiseModelPtr gaussian = + std::dynamic_pointer_cast( + this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]); + msg.set_variance(sqrt(gaussian->StdDev())); + } + } + + msg.set_pressure(diff_pressure); + msg.set_variance(temperature_local); + + // publish + this->AddSequence(msg.mutable_header()); + this->dataPtr->pub.Publish(msg); + + return true; +} + +////////////////////////////////////////////////// +gz::math::Vector3d AirSpeedSensor::Velocity() const +{ + return this->dataPtr->vel; +} + +////////////////////////////////////////////////// +void AirSpeedSensor::SetVelocity(const gz::math::Vector3d &_vel) +{ + this->dataPtr->vel = _vel; +} + +////////////////////////////////////////////////// +bool AirSpeedSensor::HasConnections() const +{ + return this->dataPtr->pub && this->dataPtr->pub.HasConnections(); +} diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index e7d771f6..7bd02827 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -137,6 +137,10 @@ gz_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimete set(air_pressure_sources AirPressureSensor.cc) gz_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target) + +set(air_speed_sources AirSpeedSensor.cc) +gz_add_component(air_speed SOURCES ${air_speed_sources} GET_TARGET_NAME air_speed_target) + set(force_torque_sources ForceTorqueSensor.cc) gz_add_component(force_torque SOURCES ${force_torque_sources} GET_TARGET_NAME force_torque_target) @@ -205,4 +209,3 @@ gz_build_tests(TYPE UNIT SOURCES ${gtest_sources} LIB_DEPS ${rendering_target}) gz_build_tests(TYPE UNIT SOURCES Lidar_TEST.cc LIB_DEPS ${lidar_target}) gz_build_tests(TYPE UNIT SOURCES Camera_TEST.cc LIB_DEPS ${camera_target}) gz_build_tests(TYPE UNIT SOURCES ImuSensor_TEST.cc LIB_DEPS ${imu_target}) - diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index 7702174b..d45e458a 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -57,6 +57,7 @@ gz_build_tests(TYPE INTEGRATION ${tests} LIB_DEPS ${PROJECT_LIBRARY_TARGET_NAME}-air_pressure + ${PROJECT_LIBRARY_TARGET_NAME}-air_speed ${PROJECT_LIBRARY_TARGET_NAME}-altimeter ${PROJECT_LIBRARY_TARGET_NAME}-force_torque ${PROJECT_LIBRARY_TARGET_NAME}-imu @@ -64,4 +65,3 @@ gz_build_tests(TYPE INTEGRATION ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer ${PROJECT_LIBRARY_TARGET_NAME}-navsat ) - From adb081ef887a7ac3534f0f7efb366e569884d31b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 28 Dec 2022 00:02:16 +0100 Subject: [PATCH 2/9] Cleanups and fixes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/AirSpeedSensor.cc | 27 +-------------------------- 1 file changed, 1 insertion(+), 26 deletions(-) diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc index 4438c248..76a78149 100644 --- a/src/AirSpeedSensor.cc +++ b/src/AirSpeedSensor.cc @@ -160,41 +160,16 @@ bool AirSpeedSensor::Update( frame->set_key("frame_id"); frame->add_value(this->FrameId()); - // // This block of code comes from RotorS: - // // https://github.com/ethz-asl/rotors_simulator/blob/master/rotors_gazebo_plugins/src/gazebo_pressure_plugin.cpp - // { - // // Get the current height. - // double height = this->dataPtr->referenceAltitude + this->Pose().Pos().Z(); - // - // // Compute the geopotential height. - // double geoHeight = kEarthRadiusMeters * height / - // (kEarthRadiusMeters + height); - // - // // Compute the temperature at the current altitude in Kelvin. - // double tempAtHeight = - // kSeaLevelTempKelvin - kTempLapseKelvinPerMeter * geoHeight; - // - // // Compute the current air pressure. - // this->dataPtr->pressure = - // kPressureOneAtmospherePascals * exp(kAirConstantDimensionless * - // log(kSeaLevelTempKelvin / tempAtHeight)); - // } - const float alt_rel = static_cast(this->Pose().Pos().Z()); // Z-component from ENU const float alt_amsl = (float)DEFAULT_HOME_ALT_AMSL + alt_rel; const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; const float density_ratio = powf(TEMPERATURE_MSL / temperature_local , 4.256f); const float air_density = AIR_DENSITY_MSL / density_ratio; - std::cerr << "air_density " << air_density << '\n'; - // // calculate differential pressure + noise in hPa gz::math::Vector3d wind_vel_{0, 0, 0}; gz::math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); gz::math::Vector3d air_vel_in_body_ = this->dataPtr->vel - veh_q_world_to_body.RotateVectorReverse(wind_vel_); double diff_pressure = gz::math::sgn(air_vel_in_body_.X()) * 0.005f * air_density * air_vel_in_body_.X() * air_vel_in_body_.X(); - std::cerr << "diff_pressure " << diff_pressure << '\n'; - std::cerr << "this->dataPtr->vel " << this->dataPtr->vel << '\n'; - std::cerr << "veh_q_world_to_body " << veh_q_world_to_body << '\n'; // Apply pressure noise if (this->dataPtr->noises.find(AIR_SPEED_NOISE_PASCALS) != @@ -214,7 +189,7 @@ bool AirSpeedSensor::Update( } } - msg.set_pressure(diff_pressure); + msg.set_pressure(diff_pressure * 100); msg.set_variance(temperature_local); // publish From 687c2652d1a97d8cc1cab2e15542d1dd8b4fc8ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 3 Feb 2023 10:06:04 +0100 Subject: [PATCH 3/9] Used air speed message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/gz/sensors/AirSpeedSensor.hh | 2 +- src/AirSpeedSensor.cc | 27 +++++++++------------------ 2 files changed, 10 insertions(+), 19 deletions(-) diff --git a/include/gz/sensors/AirSpeedSensor.hh b/include/gz/sensors/AirSpeedSensor.hh index d0afc634..bf5ceef1 100644 --- a/include/gz/sensors/AirSpeedSensor.hh +++ b/include/gz/sensors/AirSpeedSensor.hh @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc index 76a78149..ff75b05d 100644 --- a/src/AirSpeedSensor.cc +++ b/src/AirSpeedSensor.cc @@ -1,5 +1,5 @@ /* - * Copyright (C) 2019 Open Source Robotics Foundation + * 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. @@ -20,7 +20,7 @@ #pragma warning(disable: 4005) #pragma warning(disable: 4251) #endif -#include +#include #if defined(_MSC_VER) #pragma warning(pop) #endif @@ -41,7 +41,7 @@ using namespace sensors; static constexpr double kPressureOneAtmospherePascals = 101325.0; // static constexpr double kSeaLevelTempKelvin = 288.15; -static constexpr auto DEFAULT_HOME_ALT_AMSL = 488.0f; // altitude AMSL at Irchel Park, Zurich, Switzerland [m] +static constexpr auto DEFAULT_HOME_ALT_AMSL = 0.0f; // altitude AMSL see level [m] // international standard atmosphere (troposphere model - valid up to 11km) see [1] static constexpr auto TEMPERATURE_MSL = 288.15f; // temperature at MSL [K] (15 [C]) @@ -112,7 +112,7 @@ bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) this->SetTopic("/air_speed"); this->dataPtr->pub = - this->dataPtr->node.Advertise( + this->dataPtr->node.Advertise( this->Topic()); if (!this->dataPtr->pub) @@ -125,10 +125,10 @@ bool AirSpeedSensor::Load(const sdf::Sensor &_sdf) << this->Topic() << "]" << std::endl; // Load the noise parameters - if (_sdf.AirSpeedSensor()->SpeedNoise().Type() != sdf::NoiseType::NONE) + if (_sdf.AirSpeedSensor()->PressureNoise().Type() != sdf::NoiseType::NONE) { this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS] = - NoiseFactory::NewNoiseModel(_sdf.AirSpeedSensor()->SpeedNoise()); + NoiseFactory::NewNoiseModel(_sdf.AirSpeedSensor()->PressureNoise()); } this->dataPtr->initialized = true; @@ -154,7 +154,7 @@ bool AirSpeedSensor::Update( return false; } - msgs::FluidPressure msg; + msgs::AirSpeedSensor msg; *msg.mutable_header()->mutable_stamp() = msgs::Convert(_now); auto frame = msg.mutable_header()->add_data(); frame->set_key("frame_id"); @@ -178,19 +178,10 @@ bool AirSpeedSensor::Update( diff_pressure = this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply( diff_pressure); - - if (this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Type() == - NoiseType::GAUSSIAN) - { - GaussianNoiseModelPtr gaussian = - std::dynamic_pointer_cast( - this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]); - msg.set_variance(sqrt(gaussian->StdDev())); - } } - msg.set_pressure(diff_pressure * 100); - msg.set_variance(temperature_local); + msg.set_diff_pressure(diff_pressure * 100); + msg.set_temperature(temperature_local); // publish this->AddSequence(msg.mutable_header()); From f455c8be5a38c18f14233f3802f42c885ecb876b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 8 Feb 2023 09:49:48 +0100 Subject: [PATCH 4/9] added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/gz/sensors/AirSpeedSensor.hh | 6 ++-- src/AirSpeedSensor.cc | 52 ++++++++++++++++------------ 2 files changed, 33 insertions(+), 25 deletions(-) diff --git a/include/gz/sensors/AirSpeedSensor.hh b/include/gz/sensors/AirSpeedSensor.hh index bf5ceef1..c3832a4b 100644 --- a/include/gz/sensors/AirSpeedSensor.hh +++ b/include/gz/sensors/AirSpeedSensor.hh @@ -24,7 +24,7 @@ #include #include -#include +#include #include "gz/sensors/Sensor.hh" @@ -38,9 +38,9 @@ namespace gz /// \brief forward declarations class AirSpeedSensorPrivate; - /// \brief AirSensor Sensor Class + /// \brief AirSpeed Sensor Class /// - /// A sensor that reports air pressure readings. + /// A sensor that reports air speed through differential pressure readings. class AirSpeedSensor : public Sensor { diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc index ff75b05d..023c38af 100644 --- a/src/AirSpeedSensor.cc +++ b/src/AirSpeedSensor.cc @@ -38,16 +38,17 @@ using namespace gz; using namespace sensors; -static constexpr double kPressureOneAtmospherePascals = 101325.0; -// static constexpr double kSeaLevelTempKelvin = 288.15; - -static constexpr auto DEFAULT_HOME_ALT_AMSL = 0.0f; // altitude AMSL see level [m] - -// international standard atmosphere (troposphere model - valid up to 11km) see [1] -static constexpr auto TEMPERATURE_MSL = 288.15f; // temperature at MSL [K] (15 [C]) -static constexpr auto PRESSURE_MSL = 101325.0f; // pressure at MSL [Pa] -static constexpr auto LAPSE_RATE = 0.0065f; // reduction in temperature with altitude for troposphere [K/m] -static constexpr auto AIR_DENSITY_MSL = 1.225f; // air density at MSL [kg/m^3] +static constexpr auto kDefaultHomeAltAmsl = 0.0f; // altitude AMSL see level [m] + +// international standard atmosphere (troposphere model - valid up to 11km). +// temperature at MSL [K] (15 [C]) +static constexpr auto kTemperaturMsl = 288.15f; +// pressure at MSL [Pa] +static constexpr auto kPressureMsl = 101325.0f; +// reduction in temperature with altitude for troposphere [K/m] +static constexpr auto kLapseRate = 0.0065f; +// air density at MSL [kg/m^3] +static constexpr auto kAirDensityMsl = 1.225f; /// \brief Private data for AirSpeedSensor class gz::sensors::AirSpeedSensorPrivate @@ -55,7 +56,7 @@ class gz::sensors::AirSpeedSensorPrivate /// \brief node to create publisher public: transport::Node node; - /// \brief publisher to publish air pressure messages. + /// \brief publisher to publish air speed messages. public: transport::Node::Publisher pub; /// \brief true if Load() has been called and was successful @@ -64,7 +65,7 @@ class gz::sensors::AirSpeedSensorPrivate /// \brief Pressure in pascals. public: double pressure = 0.0; - /// \brief Pose of the sensor + /// \brief Velocity of the air coming from the sensor public: gz::math::Vector3d vel; /// \brief Noise added to sensor data @@ -160,16 +161,22 @@ bool AirSpeedSensor::Update( frame->set_key("frame_id"); frame->add_value(this->FrameId()); - const float alt_rel = static_cast(this->Pose().Pos().Z()); // Z-component from ENU - const float alt_amsl = (float)DEFAULT_HOME_ALT_AMSL + alt_rel; - const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; - const float density_ratio = powf(TEMPERATURE_MSL / temperature_local , 4.256f); - const float air_density = AIR_DENSITY_MSL / density_ratio; + // compute the air density at the local altitude / temperature + // Z-component from ENU + const float alt_rel = static_cast(this->Pose().Pos().Z()); + const float alt_amsl = kDefaultHomeAltAmsl + alt_rel; + const float temperature_local = kTemperaturMsl - kLapseRate * alt_amsl; + const float density_ratio = powf(kTemperaturMsl / temperature_local , 4.256f); + const float air_density = kAirDensityMsl / density_ratio; + + math::Vector3d wind_vel_{0, 0, 0}; + math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); - gz::math::Vector3d wind_vel_{0, 0, 0}; - gz::math::Quaterniond veh_q_world_to_body = this->Pose().Rot(); - gz::math::Vector3d air_vel_in_body_ = this->dataPtr->vel - veh_q_world_to_body.RotateVectorReverse(wind_vel_); - double diff_pressure = gz::math::sgn(air_vel_in_body_.X()) * 0.005f * air_density * air_vel_in_body_.X() * air_vel_in_body_.X(); + // calculate differential pressure + noise in hPa + math::Vector3d air_vel_in_body_ = this->dataPtr->vel - + veh_q_world_to_body.RotateVectorReverse(wind_vel_); + float diff_pressure = math::sgn(air_vel_in_body_.X()) * 0.005f * air_density + * air_vel_in_body_.X() * air_vel_in_body_.X(); // Apply pressure noise if (this->dataPtr->noises.find(AIR_SPEED_NOISE_PASCALS) != @@ -178,9 +185,10 @@ bool AirSpeedSensor::Update( diff_pressure = this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply( diff_pressure); + msg.set_pressure_noise(this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]); } - msg.set_diff_pressure(diff_pressure * 100); + msg.set_diff_pressure(diff_pressure * 100.0f); msg.set_temperature(temperature_local); // publish From 83b9c8db4f1e652e97d082c63050bc07387d8d01 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 8 Feb 2023 10:13:19 +0100 Subject: [PATCH 5/9] Added test MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/AirSpeedSensor.cc | 2 +- test/integration/CMakeLists.txt | 1 + test/integration/air_speed.cc | 258 ++++++++++++++++++++++++++++++++ 3 files changed, 260 insertions(+), 1 deletion(-) create mode 100644 test/integration/air_speed.cc diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc index 023c38af..bb02f4a7 100644 --- a/src/AirSpeedSensor.cc +++ b/src/AirSpeedSensor.cc @@ -185,7 +185,7 @@ bool AirSpeedSensor::Update( diff_pressure = this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply( diff_pressure); - msg.set_pressure_noise(this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]); + msg.mutable_pressure_noise()->set_type(msgs::SensorNoise::GAUSSIAN); } msg.set_diff_pressure(diff_pressure * 100.0f); diff --git a/test/integration/CMakeLists.txt b/test/integration/CMakeLists.txt index d45e458a..aaa5b908 100644 --- a/test/integration/CMakeLists.txt +++ b/test/integration/CMakeLists.txt @@ -15,6 +15,7 @@ set(dri_tests set(tests air_pressure.cc + air_speed.cc altimeter.cc force_torque.cc imu.cc diff --git a/test/integration/air_speed.cc b/test/integration/air_speed.cc new file mode 100644 index 00000000..9c370cf1 --- /dev/null +++ b/test/integration/air_speed.cc @@ -0,0 +1,258 @@ +/* + * 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 "test_config.hh" // NOLINT(build/include) +#include "TransportTestTools.hh" + +/// \brief Helper function to create an air pressure sdf element +sdf::ElementPtr AirSpeedToSdf(const std::string &_name, + const gz::math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +/// \brief Helper function to create an air pressure sdf element with noise +sdf::ElementPtr AirSpeedToSdfWithNoise(const std::string &_name, + const gz::math::Pose3d &_pose, const double _updateRate, + const std::string &_topic, const bool _alwaysOn, + const bool _visualize, double _mean, double _stddev, double _bias) +{ + std::ostringstream stream; + stream + << "" + << "" + << " " + << " " + << " " + << " " << _pose << "" + << " " << _topic << "" + << " "<< _updateRate <<"" + << " " << _alwaysOn <<"" + << " " << _visualize << "" + << " " + << " " + << " " + << " " << _mean << "" + << " " << _stddev << "" + << " " << _bias << "" + << " " + << " " + << " " + << " " + << " " + << " " + << ""; + + sdf::SDFPtr sdfParsed(new sdf::SDF()); + sdf::init(sdfParsed); + if (!sdf::readString(stream.str(), sdfParsed)) + return sdf::ElementPtr(); + + return sdfParsed->Root()->GetElement("model")->GetElement("link") + ->GetElement("sensor"); +} + +/// \brief Test air pressure sensor +class AirSpeedSensorTest: public testing::Test +{ + // Documentation inherited + protected: void SetUp() override + { + gz::common::Console::SetVerbosity(4); + } +}; + +///////////////////////////////////////////////// +TEST_F(AirSpeedSensorTest, CreateAirSpeed) +{ + // Create SDF describing an air_pressure sensor + const std::string name = "TestAirSpeed"; + const std::string topic = "/gz/sensors/test/air_speed"; + const std::string topicNoise = "/gz/sensors/test/air_speed_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); + sdf::ElementPtr airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + sdf::ElementPtr airSpeedSdfNoise = AirSpeedToSdfWithNoise(name, + sensorPose, updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + gz::sensors::SensorFactory sf; + std::unique_ptr sensor = + sf.CreateSensor(airSpeedSdf); + ASSERT_NE(nullptr, sensor); + + EXPECT_EQ(name, sensor->Name()); + EXPECT_EQ(topic, sensor->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensor->UpdateRate()); + + std::unique_ptr sensorNoise = + sf.CreateSensor( + airSpeedSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + EXPECT_EQ(name, sensorNoise->Name()); + EXPECT_EQ(topicNoise, sensorNoise->Topic()); + EXPECT_DOUBLE_EQ(updateRate, sensorNoise->UpdateRate()); +} + +///////////////////////////////////////////////// +TEST_F(AirSpeedSensorTest, SensorReadings) +{ + // Create SDF describing an air_pressure sensor + const std::string name = "TestAirSpeed"; + const std::string topic = "/gz/sensors/test/air_pressure"; + const std::string topicNoise = "/gz/sensors/test/air_pressure_noise"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + + // Create sensor SDF + gz::math::Pose3d sensorPose(gz::math::Vector3d(0.25, 0.0, 0.5), + gz::math::Quaterniond::Identity); + sdf::ElementPtr airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + sdf::ElementPtr airSpeedSdfNoise = AirSpeedToSdfWithNoise(name, + sensorPose, updateRate, topicNoise, alwaysOn, visualize, 1.0, 0.2, 10.0); + + // create the sensor using sensor factory + gz::sensors::SensorFactory sf; + auto sensor = sf.CreateSensor( + airSpeedSdf); + ASSERT_NE(nullptr, sensor); + EXPECT_FALSE(sensor->HasConnections()); + + auto sensorNoise = sf.CreateSensor( + airSpeedSdfNoise); + ASSERT_NE(nullptr, sensorNoise); + + sensor->SetPose( + gz::math::Pose3d(0, 0, 1.5, 0, 0, 0) * sensorNoise->Pose()); + + // verify msg received on the topic + WaitForMessageTestHelper msgHelper(topic); + EXPECT_TRUE(sensor->HasConnections()); + sensor->Update(std::chrono::steady_clock::duration(std::chrono::seconds(1))); + EXPECT_TRUE(msgHelper.WaitForMessage()) << msgHelper; + auto msg = msgHelper.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + EXPECT_DOUBLE_EQ(0.0, msg.diff_pressure()); + EXPECT_DOUBLE_EQ(288.1369934082031, msg.temperature()); + + // verify msg with noise received on the topic + WaitForMessageTestHelper + msgHelperNoise(topicNoise); + sensorNoise->Update(std::chrono::steady_clock::duration( + std::chrono::seconds(1)), false); + EXPECT_TRUE(msgHelperNoise.WaitForMessage()) << msgHelperNoise; + auto msgNoise = msgHelperNoise.Message(); + EXPECT_EQ(1, msg.header().stamp().sec()); + EXPECT_EQ(0, msg.header().stamp().nsec()); + EXPECT_FALSE(gz::math::equal(0.0, msgNoise.diff_pressure())); + EXPECT_DOUBLE_EQ(288.14675903320312, msgNoise.temperature()); +} + +///////////////////////////////////////////////// +TEST_F(AirSpeedSensorTest, Topic) +{ + const std::string name = "TestAirSpeed"; + const double updateRate = 30; + const bool alwaysOn = 1; + const bool visualize = 1; + auto sensorPose = gz::math::Pose3d(); + + // Factory + gz::sensors::SensorFactory factory; + + // Default topic + { + const std::string topic; + auto airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto airSpeed = factory.CreateSensor< + gz::sensors::AirSpeedSensor>(airSpeedSdf); + ASSERT_NE(nullptr, airSpeed); + + EXPECT_EQ("/air_speed", airSpeed->Topic()); + } + + // Convert to valid topic + { + const std::string topic = "/topic with spaces/@~characters//"; + auto airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto airSpeed = factory.CreateSensor< + gz::sensors::AirSpeedSensor>(airSpeedSdf); + ASSERT_NE(nullptr, airSpeed); + + EXPECT_EQ("/topic_with_spaces/characters", airSpeed->Topic()); + } + + // Invalid topic + { + const std::string topic = "@@@"; + auto airSpeedSdf = AirSpeedToSdf(name, sensorPose, + updateRate, topic, alwaysOn, visualize); + + auto sensor = factory.CreateSensor< + gz::sensors::AirSpeedSensor>(airSpeedSdf); + ASSERT_EQ(nullptr, sensor); + } +} From 6f5ee57869bdc4888b57391cf6710355c635d822 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 8 Feb 2023 10:13:57 +0100 Subject: [PATCH 6/9] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/AirSpeedSensor.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/AirSpeedSensor.cc b/src/AirSpeedSensor.cc index bb02f4a7..741274ff 100644 --- a/src/AirSpeedSensor.cc +++ b/src/AirSpeedSensor.cc @@ -38,8 +38,8 @@ using namespace gz; using namespace sensors; -static constexpr auto kDefaultHomeAltAmsl = 0.0f; // altitude AMSL see level [m] - +// altitude AMSL see level [m] +static constexpr auto kDefaultHomeAltAmsl = 0.0f; // international standard atmosphere (troposphere model - valid up to 11km). // temperature at MSL [K] (15 [C]) static constexpr auto kTemperaturMsl = 288.15f; From ef8c157e18c11e11175b2468d59e47507395bff4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 8 Feb 2023 10:17:48 +0100 Subject: [PATCH 7/9] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- test/integration/air_speed.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/integration/air_speed.cc b/test/integration/air_speed.cc index 9c370cf1..1b860c59 100644 --- a/test/integration/air_speed.cc +++ b/test/integration/air_speed.cc @@ -244,7 +244,7 @@ TEST_F(AirSpeedSensorTest, Topic) EXPECT_EQ("/topic_with_spaces/characters", airSpeed->Topic()); } - + // Invalid topic { const std::string topic = "@@@"; From 857a9be276c71baafecfa254d284a279a3ddf111 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Wed, 8 Feb 2023 17:01:15 +0100 Subject: [PATCH 8/9] gz-msgs version MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index af7e1963..ff5634de 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -84,7 +84,7 @@ endif() #-------------------------------------- # Find gz-msgs -gz_find_package(gz-msgs9 REQUIRED) +gz_find_package(gz-msgs9 REQUIRED VERSION 9.3) set(GZ_MSGS_VER ${gz-msgs9_VERSION_MAJOR}) #-------------------------------------- From fe3abbcb94c7b6ddb0702d3c74b624c2ef0db732 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 9 Feb 2023 23:49:39 +0100 Subject: [PATCH 9/9] Fixed windows build and minor fixes MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- include/gz/sensors/AirSpeedSensor.hh | 2 +- test/integration/air_speed.cc | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/gz/sensors/AirSpeedSensor.hh b/include/gz/sensors/AirSpeedSensor.hh index c3832a4b..a27fcb83 100644 --- a/include/gz/sensors/AirSpeedSensor.hh +++ b/include/gz/sensors/AirSpeedSensor.hh @@ -41,7 +41,7 @@ namespace gz /// \brief AirSpeed Sensor Class /// /// A sensor that reports air speed through differential pressure readings. - class AirSpeedSensor : + class GZ_SENSORS_AIR_SPEED_VISIBLE AirSpeedSensor : public Sensor { /// \brief constructor diff --git a/test/integration/air_speed.cc b/test/integration/air_speed.cc index 1b860c59..59b0d630 100644 --- a/test/integration/air_speed.cc +++ b/test/integration/air_speed.cc @@ -26,7 +26,7 @@ #include "test_config.hh" // NOLINT(build/include) #include "TransportTestTools.hh" -/// \brief Helper function to create an air pressure sdf element +/// \brief Helper function to create an air speed sdf element sdf::ElementPtr AirSpeedToSdf(const std::string &_name, const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, @@ -58,7 +58,7 @@ sdf::ElementPtr AirSpeedToSdf(const std::string &_name, ->GetElement("sensor"); } -/// \brief Helper function to create an air pressure sdf element with noise +/// \brief Helper function to create an air speed sdf element with noise sdf::ElementPtr AirSpeedToSdfWithNoise(const std::string &_name, const gz::math::Pose3d &_pose, const double _updateRate, const std::string &_topic, const bool _alwaysOn, @@ -99,7 +99,7 @@ sdf::ElementPtr AirSpeedToSdfWithNoise(const std::string &_name, ->GetElement("sensor"); } -/// \brief Test air pressure sensor +/// \brief Test air speed sensor class AirSpeedSensorTest: public testing::Test { // Documentation inherited