Windsensor Model

.gitignore

@@ -1,5 +1,6 @@
 Build/
 build/
+.vscode/
 scripts/schemas
 .DS_Store
 *~

CMakeLists.txt

@@ -374,8 +374,9 @@ add_library(gazebo_barometer_plugin SHARED src/gazebo_barometer_plugin.cpp)
 add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp)
 add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp)
 add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp)
-add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp)
 add_library(gazebo_drop_plugin SHARED src/gazebo_drop_plugin.cpp)
+add_library(gazebo_airflowsensor_plugin SHARED src/gazebo_airflowsensor_plugin.cpp)
+add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp)
 
 set(plugins
   gazebo_airspeed_plugin
@@ -401,8 +402,9 @@ set(plugins
   gazebo_catapult_plugin
   gazebo_usv_dynamics_plugin
   gazebo_parachute_plugin
-  gazebo_airship_dynamics_plugin
   gazebo_drop_plugin
+  gazebo_airflowsensor_plugin
+  gazebo_airship_dynamics_plugin
   )
 
 foreach(plugin ${plugins})

include/gazebo_airflowsensor_plugin.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2020 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+/**
+ * @brief AirflowSensor Plugin
+ *
+ * This plugin publishes Airflow sensor data
+ *
+ * @author Henry Kotze <[email protected]>
+ */
+
+#ifndef _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_
+#define _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_
+
+#include <math.h>
+#include <cstdio>
+#include <cstdlib>
+#include <queue>
+#include <random>
+
+#include <sdf/sdf.hh>
+#include <common.h>
+#include <random>
+
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/util/system.hh>
+#include <gazebo/transport/transport.hh>
+#include <gazebo/msgs/msgs.hh>
+#include <gazebo/physics/physics.hh>
+#include <ignition/math.hh>
+
+#include <gazebo/sensors/SensorTypes.hh>
+#include <gazebo/sensors/Sensor.hh>
+
+#include <Airspeed.pb.h>
+#include <Wind.pb.h>
+
+namespace gazebo
+{
+
+typedef const boost::shared_ptr<const physics_msgs::msgs::Wind> WindPtr;
+
+class GAZEBO_VISIBLE AirflowSensorPlugin : public SensorPlugin
+{
+public:
+  AirflowSensorPlugin();
+  virtual ~AirflowSensorPlugin();
+
+protected:
+  virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
+  virtual void OnUpdate(const common::UpdateInfo&);
+  virtual void OnSensorUpdate();
+
+private:
+  void WindVelocityCallback(WindPtr& msg);
+
+  physics::ModelPtr model_;
+  physics::WorldPtr world_;
+  physics::LinkPtr link_;
+  sensors::SensorPtr parentSensor_;
+
+  transport::NodePtr node_handle_;
+  transport::SubscriberPtr wind_sub_;
+  transport::PublisherPtr airflow_sensor_pub_;
+  event::ConnectionPtr updateSensorConnection_;
+  event::ConnectionPtr updateConnection_;
+
+  // linear velocity
+  ignition::math::Vector3d vel_a_;
+  // angular velocity
+  ignition::math::Vector3d ang_a_;
+
+  common::Time last_time_;
+  std::string namespace_;
+  std::string link_name_;
+  std::string model_name_;
+  std::string airflowsensor_topic_;
+
+  std::normal_distribution<float> gauss_dir_;
+  std::normal_distribution<float> gauss_speed_;
+  std::default_random_engine generator_;
+
+  ignition::math::Vector3d wind_;
+  ignition::math::Vector3d body_wind_;
+  ignition::math::Vector3d body_vel_;
+  float airflow_direction_;
+  float airflow_speed_;
+
+};     // class GAZEBO_VISIBLE AirflowSensorPlugin
+}      // namespace gazebo
+#endif // _GAZEBO_AIRFLOWSENSOR_PLUGIN_HH_

include/gazebo_mavlink_interface.h

@@ -80,6 +80,7 @@ static const std::regex kDefaultGPSModelNaming(".*(gps|ublox-neo-7M)(.*)");
 static const std::regex kDefaultAirspeedModelJointNaming(".*(airspeed)(.*_joint)");
 static const std::regex kDefaultImuModelJointNaming(".*(imu)(\\d*_joint)");
 static const std::regex kDefaultMagModelJointNaming(".*(mag)(\\d*_joint)");
+static const std::regex kDefaultAirflowSensorModelJointNaming(".*(airflowsensor)(.*_joint)");
 
 namespace gazebo {
 

include/mavlink_interface.h

@@ -81,7 +81,7 @@ enum class SensorSource {
   GYRO		= 0b111000,
   MAG		= 0b111000000,
   BARO		= 0b1101000000000,
-  DIFF_PRESS	= 0b10000000000,
+  DIFF_PRESS	= 0b10000000000
 };
 
 namespace SensorData {
@@ -104,6 +104,13 @@ namespace SensorData {
 
     struct Airspeed {
         double diff_pressure;
+        double speed;
+        double direction;
+    };
+
+    struct WindSensor {
+        double wind_speed;
+        double wind_direction;
     };
 
     struct Gps {
@@ -129,12 +136,15 @@ struct HILData {
     int id=-1;
     bool baro_updated{false};
     bool diff_press_updated{false};
+    bool airflow_updated{false};
     bool mag_updated{false};
     bool imu_updated{false};
     double temperature;
     double pressure_alt;
     double abs_pressure;
     double diff_pressure;
+    double airflow_speed;
+    double airflow_direction;
     Eigen::Vector3d mag_b;
     Eigen::Vector3d accel_b;
     Eigen::Vector3d gyro_b;
@@ -155,6 +165,8 @@ class MavlinkInterface {
     void SendHeartbeat();
     void SendSensorMessages(const uint64_t time_usec);
     void SendSensorMessages(const uint64_t time_usec, HILData &hil_data);
+    void SendAirflowSensorMessages(uint64_t time_usec, HILData &hil_data);
+
     void SendGpsMessages(const SensorData::Gps &data);
     void UpdateBarometer(const SensorData::Barometer &data, const int id = 0);
     void UpdateAirspeed(const SensorData::Airspeed &data, const int id = 0);

models/airflowsensor/airflowsensor.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="airflowsensor">
+    <link name="link">
+      <pose>0 0 0 0 0 0</pose>
+      <visual name="visual">
+        <geometry>
+          <cylinder>
+            <radius>0.01</radius>
+            <length>0.1</length>
+          </cylinder>
+        </geometry>
+        <material>
+          <script>
+            <name>Gazebo/Black</name>
+          </script>
+        </material>
+      </visual>
+      <sensor name='airflowsensor' type='gps'>
+        <pose>0 0 0 0 0 0</pose>
+        <update_rate>5.0</update_rate>
+        <always_on>true</always_on>
+        <visualize>false</visualize>
+        <plugin name='gazebo_airflowsensor_plugin' filename='libgazebo_airflowsensor_plugin.so'>
+          <robotNamespace/>
+        </plugin>
+      </sensor>
+    </link>
+  </model>
+</sdf>

models/airflowsensor/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>airflowsensor</name>
+  <version>1.0</version>
+  <sdf version='1.6'>airflowsensor.sdf</sdf>
+
+  <author>
+   <name>Henry Kotze</name>
+   <email>[email protected]</email>
+  </author>
+
+  <description>
+    FT Technologies ultra sonic wind sensor model
+  </description>
+</model>