Airflow/Wind sensor

include/gz/sensors/AirFlowSensor.hh

@@ -0,0 +1,101 @@
+/*
+ * 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_AIRFLOWSENSOR_HH_
+#define GZ_SENSORS_AIRFLOWSENSOR_HH_
+
+#include <memory>
+
+#include <sdf/sdf.hh>
+
+#include <gz/utils/SuppressWarning.hh>
+
+#include <gz/sensors/config.hh>
+#include <gz/sensors/air_flow/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 AirFlowSensorPrivate;
+
+    /// \brief AirFlow Sensor Class
+    ///
+    /// A sensor that reports air speed through differential pressure readings.
+    class GZ_SENSORS_AIR_FLOW_VISIBLE AirFlowSensor :
+      public Sensor
+    {
+      /// \brief constructor
+      public: AirFlowSensor();
+
+      /// \brief destructor
+      public: virtual ~AirFlowSensor();
+
+      /// \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
+      /// \param[in] _vel The velocity of the sensor [m/s]
+      public: void SetVelocity(const gz::math::Vector3d &_vel);
+
+      /// \brief Update the wind velocity
+      /// \param[in] _vel The wind velocity [m/s]
+      public: void SetWindVelocity(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<AirFlowSensorPrivate> dataPtr;
+      GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+    };
+    }
+  }
+}
+
+#endif

include/gz/sensors/SensorTypes.hh

@@ -209,6 +209,14 @@ namespace gz
       /// \sa AirSpeedSensor
       AIR_SPEED_NOISE_PASCALS = 25,
 
+      /// \brief Air flow noise streams for the air flow sensor
+      /// \sa AirFlowSensor
+      AIR_FLOW_SPEED_NOISE = 26,
+
+      /// \brief Air flow noise streams for the air flow sensor
+      /// \sa AirFlowSensor
+      AIR_FLOW_DIR_NOISE = 27,
+
       /// \internal
       /// \brief Indicator used to create an iterator over the enum. Do not
       /// use this.

src/AirFlowSensor.cc

@@ -0,0 +1,227 @@
+/*
+ * 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_flow_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/AirFlowSensor.hh"
+
+using namespace gz;
+using namespace sensors;
+
+/// \brief Private data for AirFlowSensor
+class gz::sensors::AirFlowSensorPrivate
+{
+  /// \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 Velocity of the sensor
+  public: gz::math::Vector3d vel;
+
+  /// \brief Velocity of the wind
+  public: gz::math::Vector3d wind_vel;
+
+  /// \brief Noise added to speed measurement
+  public: std::map<SensorNoiseType, NoisePtr> speed_noises;
+
+  /// \brief Noise added to directional measurement
+  public: std::map<SensorNoiseType, NoisePtr> dir_noises;
+};
+
+//////////////////////////////////////////////////
+AirFlowSensor::AirFlowSensor()
+  : dataPtr(new AirFlowSensorPrivate())
+{
+}
+
+//////////////////////////////////////////////////
+AirFlowSensor::~AirFlowSensor()
+{
+}
+
+//////////////////////////////////////////////////
+bool AirFlowSensor::Init()
+{
+  return this->Sensor::Init();
+}
+
+//////////////////////////////////////////////////
+bool AirFlowSensor::Load(const sdf::Sensor &_sdf)
+{
+  if (!Sensor::Load(_sdf))
+    return false;
+
+  if (_sdf.Type() != sdf::SensorType::AIR_FLOW)
+  {
+    gzerr << "Attempting to a load an AirFlow sensor, but received "
+      << "a " << _sdf.TypeStr() << std::endl;
+    return false;
+  }
+
+  if (_sdf.AirFlowSensor() == nullptr)
+  {
+    gzerr << "Attempting to a load an AirFlow sensor, but received "
+      << "a null sensor." << std::endl;
+    return false;
+  }
+
+  if (this->Topic().empty())
+    this->SetTopic("/air_flow");
+
+  this->dataPtr->pub =
+      this->dataPtr->node.Advertise<msgs::AirFlowSensor>(
+      this->Topic());
+
+  if (!this->dataPtr->pub)
+  {
+    gzerr << "Unable to create publisher on topic[" << this->Topic() << "].\n";
+    return false;
+  }
+
+  gzdbg << "Airflow for [" << this->Name() << "] advertised on ["
+         << this->Topic() << "]" << std::endl;
+
+  // Load the noise parameters
+  if (_sdf.AirFlowSensor()->SpeedNoise().Type() != sdf::NoiseType::NONE)
+  {
+    this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE] =
+      NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->SpeedNoise());
+  }
+
+  if (_sdf.AirFlowSensor()->DirectionNoise().Type() != sdf::NoiseType::NONE)
+  {
+    this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE] =
+      NoiseFactory::NewNoiseModel(_sdf.AirFlowSensor()->DirectionNoise());
+  }
+
+  this->dataPtr->initialized = true;
+  return true;
+}
+
+//////////////////////////////////////////////////
+bool AirFlowSensor::Load(sdf::ElementPtr _sdf)
+{
+  sdf::Sensor sdfSensor;
+  sdfSensor.Load(_sdf);
+  return this->Load(sdfSensor);
+}
+
+//////////////////////////////////////////////////
+bool AirFlowSensor::Update(
+  const std::chrono::steady_clock::duration &_now)
+{
+  GZ_PROFILE("AirFlowSensor::Update");
+  if (!this->dataPtr->initialized)
+  {
+    gzerr << "Not initialized, update ignored.\n";
+    return false;
+  }
+
+  msgs::AirFlowSensor 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());
+
+  math::Vector3d wind_vel_ = this->dataPtr->wind_vel;
+  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_);
+
+  // airflow flowing in to the sensor is measured as 0. Hense the substraction
+  // of M_2_PI.
+  double airflow_direction_in_xy_plane = atan2f(air_vel_in_body_.Y(),
+                                              air_vel_in_body_.X()) - M_PI_2;
+
+  // Apply noise
+  if (this->dataPtr->dir_noises.find(AIR_FLOW_DIR_NOISE) !=
+      this->dataPtr->dir_noises.end())
+  {
+    airflow_direction_in_xy_plane =
+      this->dataPtr->dir_noises[AIR_FLOW_DIR_NOISE]->Apply(
+          airflow_direction_in_xy_plane);
+    msg.mutable_direction_noise()->set_type(msgs::SensorNoise::GAUSSIAN);
+  }
+
+  air_vel_in_body_.Z() = 0;
+  double airflow_speed =  air_vel_in_body_.Length();
+
+  // Apply noise
+  if (this->dataPtr->speed_noises.find(AIR_FLOW_SPEED_NOISE) !=
+      this->dataPtr->speed_noises.end())
+  {
+    airflow_speed =
+      this->dataPtr->speed_noises[AIR_FLOW_SPEED_NOISE]->Apply(
+          airflow_speed);
+    msg.mutable_speed_noise()->set_type(msgs::SensorNoise::GAUSSIAN);
+  }
+
+  msg.set_xy_direction(airflow_direction_in_xy_plane);
+  msg.set_xy_speed(airflow_speed);
+
+  // publish
+  this->AddSequence(msg.mutable_header());
+  this->dataPtr->pub.Publish(msg);
+
+  return true;
+}
+
+//////////////////////////////////////////////////
+gz::math::Vector3d AirFlowSensor::Velocity() const
+{
+  return this->dataPtr->vel;
+}
+
+//////////////////////////////////////////////////
+void AirFlowSensor::SetVelocity(const gz::math::Vector3d &_vel)
+{
+  this->dataPtr->vel = _vel;
+}
+
+void AirFlowSensor::SetWindVelocity(const gz::math::Vector3d &_vel)
+{
+  this->dataPtr->wind_vel = _vel;
+}
+
+//////////////////////////////////////////////////
+bool AirFlowSensor::HasConnections() const
+{
+  return this->dataPtr->pub && this->dataPtr->pub.HasConnections();
+}

src/CMakeLists.txt

@@ -135,6 +135,9 @@ gz_add_component(imu SOURCES ${imu_sources} GET_TARGET_NAME imu_target)
 set(altimeter_sources AltimeterSensor.cc)
 gz_add_component(altimeter SOURCES ${altimeter_sources} GET_TARGET_NAME altimeter_target)
 
+set(air_flow_sources AirFlowSensor.cc)
+gz_add_component(air_flow SOURCES ${air_flow_sources} GET_TARGET_NAME air_flow_target)
+
 set(air_pressure_sources AirPressureSensor.cc)
 gz_add_component(air_pressure SOURCES ${air_pressure_sources} GET_TARGET_NAME air_pressure_target)
 

test/integration/CMakeLists.txt

@@ -15,6 +15,7 @@ set(dri_tests
 )
 
 set(tests
+  air_flow.cc
   air_pressure.cc
   air_speed.cc
   altimeter.cc
@@ -58,6 +59,7 @@ gz_build_tests(TYPE INTEGRATION
   SOURCES
     ${tests}
   LIB_DEPS
+    ${PROJECT_LIBRARY_TARGET_NAME}-air_flow
     ${PROJECT_LIBRARY_TARGET_NAME}-air_pressure
     ${PROJECT_LIBRARY_TARGET_NAME}-air_speed
     ${PROJECT_LIBRARY_TARGET_NAME}-altimeter