Added airspeed sensor

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})
 
 #--------------------------------------

include/gz/sensors/AirSpeedSensor.hh

@@ -0,0 +1,96 @@
+/*
+ * 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_SENSORS_AIRSPEEDSENSOR_HH_
+#define GZ_SENSORS_AIRSPEEDSENSOR_HH_
+
+#include <memory>
+
+#include <sdf/sdf.hh>
+
+#include <gz/utils/SuppressWarning.hh>
+
+#include <gz/sensors/config.hh>
+#include <gz/sensors/air_speed/Export.hh>
+
+#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 AirSpeed Sensor Class
+    ///
+    /// A sensor that reports air speed through differential pressure readings.
+    class GZ_SENSORS_AIR_SPEED_VISIBLE 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<AirSpeedSensorPrivate> dataPtr;
+      GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+    };
+    }
+  }
+}
+
+#endif

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.

src/AirSpeedSensor.cc

@@ -0,0 +1,217 @@
+/*
+ * 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.
+ *
+*/
+
+#if defined(_MSC_VER)
+  #pragma warning(push)
+  #pragma warning(disable: 4005)
+  #pragma warning(disable: 4251)
+#endif
+#include <gz/msgs/air_speed_sensor.pb.h>
+#if defined(_MSC_VER)
+  #pragma warning(pop)
+#endif
+#include <gz/msgs/Utility.hh>
+
+#include <gz/common/Profiler.hh>
+#include <gz/transport/Node.hh>
+
+#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;
+
+// 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;
+// 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
+{
+  /// \brief node to create publisher
+  public: transport::Node node;
+
+  /// \brief publisher to publish air speed 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 Velocity of the air coming from the sensor
+  public: gz::math::Vector3d vel;
+
+  /// \brief Noise added to sensor data
+  public: std::map<SensorNoiseType, NoisePtr> 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<msgs::AirSpeedSensor>(
+      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()->PressureNoise().Type() != sdf::NoiseType::NONE)
+  {
+    this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS] =
+      NoiseFactory::NewNoiseModel(_sdf.AirSpeedSensor()->PressureNoise());
+  }
+
+  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::AirSpeedSensor 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());
+
+  // compute the air density at the local altitude / temperature
+  // Z-component from ENU
+  const float alt_rel = static_cast<float>(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();
+
+  // 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) !=
+      this->dataPtr->noises.end())
+  {
+    diff_pressure =
+      this->dataPtr->noises[AIR_SPEED_NOISE_PASCALS]->Apply(
+          diff_pressure);
+    msg.mutable_pressure_noise()->set_type(msgs::SensorNoise::GAUSSIAN);
+  }
+
+  msg.set_diff_pressure(diff_pressure * 100.0f);
+  msg.set_temperature(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();
+}

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})
-

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
@@ -57,11 +58,11 @@ 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
     ${PROJECT_LIBRARY_TARGET_NAME}-logical_camera
     ${PROJECT_LIBRARY_TARGET_NAME}-magnetometer
     ${PROJECT_LIBRARY_TARGET_NAME}-navsat
 )
-