Windsensor Model

.gitignore

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

CMakeLists.txt

@@ -330,6 +330,7 @@ set(sensor_msgs
   msgs/OpticalFlow.proto
   msgs/MagneticField.proto
   msgs/Pressure.proto
+  msgs/WindSensor.proto
   )
 
 PROTOBUF_GENERATE_CPP(MAV_PROTO_SRCS MAV_PROTO_HDRS ${mav_msgs})
@@ -374,8 +375,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_windsensor_plugin SHARED src/gazebo_windsensor_plugin.cpp)
+add_library(gazebo_airship_dynamics_plugin SHARED src/gazebo_airship_dynamics_plugin.cpp)
 
 set(plugins
   gazebo_airspeed_plugin
@@ -401,8 +403,9 @@ set(plugins
   gazebo_catapult_plugin
   gazebo_usv_dynamics_plugin
   gazebo_parachute_plugin
-  gazebo_airship_dynamics_plugin
   gazebo_drop_plugin
+  gazebo_windsensor_plugin
+  gazebo_airship_dynamics_plugin
   )
 
 foreach(plugin ${plugins})

include/gazebo_mavlink_interface.h

@@ -57,6 +57,7 @@
 #include <sdf/sdf.hh>
 #include <common.h>
 #include <Airspeed.pb.h>
+#include <WindSensor.pb.h>
 #include <CommandMotorSpeed.pb.h>
 #include <MotorSpeed.pb.h>
 #include <Imu.pb.h>
@@ -78,6 +79,7 @@ static const std::regex kDefaultLidarModelNaming(".*(lidar|sf10a)(.*)");
 static const std::regex kDefaultSonarModelNaming(".*(sonar|mb1240-xl-ez4)(.*)");
 static const std::regex kDefaultGPSModelNaming(".*(gps|ublox-neo-7M)(.*)");
 static const std::regex kDefaultAirspeedModelJointNaming(".*(airspeed)(.*_joint)");
+static const std::regex kDefaultWindSensorModelJointNaming(".*(windsensor)(.*_joint)");
 static const std::regex kDefaultImuModelJointNaming(".*(imu)(\\d*_joint)");
 static const std::regex kDefaultMagModelJointNaming(".*(mag)(\\d*_joint)");
 
@@ -86,6 +88,7 @@ namespace gazebo {
 typedef const boost::shared_ptr<const mav_msgs::msgs::CommandMotorSpeed> CommandMotorSpeedPtr;
 typedef const boost::shared_ptr<const nav_msgs::msgs::Odometry> OdomPtr;
 typedef const boost::shared_ptr<const sensor_msgs::msgs::Airspeed> AirspeedPtr;
+typedef const boost::shared_ptr<const sensor_msgs::msgs::WindSensor> WindSensorPtr;
 typedef const boost::shared_ptr<const sensor_msgs::msgs::Groundtruth> GtPtr;
 typedef const boost::shared_ptr<const sensor_msgs::msgs::Imu> ImuPtr;
 typedef const boost::shared_ptr<const sensor_msgs::msgs::IRLock> IRLockPtr;
@@ -179,6 +182,7 @@ class GazeboMavlinkInterface : public ModelPlugin {
   void LidarCallback(LidarPtr& lidar_msg, const int& id);
   void SonarCallback(SonarPtr& sonar_msg, const int& id);
   void AirspeedCallback(AirspeedPtr& airspeed_msg, const int& id);
+  void WindSensorCallback(WindSensorPtr& windsensor_msg, const int& id);
   void OpticalFlowCallback(OpticalFlowPtr& opticalFlow_msg);
   void IRLockCallback(IRLockPtr& irlock_msg);
   void VisionCallback(OdomPtr& odom_msg);

include/gazebo_windsensor_plugin.h

@@ -0,0 +1,119 @@
+/****************************************************************************
+ *
+ *   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 WindSensor Plugin
+ *
+ * This plugin publishes WindSensor sensor data
+ *
+ * @author Henry Kotze <[email protected]>
+ */
+
+#ifndef _GAZEBO_WINDSENSOR_PLUGIN_HH_
+#define _GAZEBO_WINDSENSOR_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 <WindSensor.pb.h>
+#include <Wind.pb.h>
+
+namespace gazebo
+{
+
+typedef const boost::shared_ptr<const physics_msgs::msgs::Wind> WindPtr;
+
+class GAZEBO_VISIBLE WindSensorPlugin : public SensorPlugin
+{
+public:
+  WindSensorPlugin();
+  virtual ~WindSensorPlugin();
+
+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 windsensor_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 windsensor_topic_;
+
+  std::normal_distribution<float> gauss_dir_;
+  std::normal_distribution<float> gauss_speed_;
+  std::default_random_engine generator_;
+
+  ignition::math::Vector3d wind_;
+  float wind_direction_;
+  float wind_speed_;
+
+};     // class GAZEBO_VISIBLE WindSensorPlugin
+}      // namespace gazebo
+#endif // _GAZEBO_WINDSENSOR_PLUGIN_HH_

include/mavlink_interface.h

@@ -82,6 +82,7 @@ enum class SensorSource {
   MAG		= 0b111000000,
   BARO		= 0b1101000000000,
   DIFF_PRESS	= 0b10000000000,
+  WIND_SENSOR	= 0b10000000000000
 };
 
 namespace SensorData {
@@ -106,6 +107,11 @@ namespace SensorData {
         double diff_pressure;
     };
 
+    struct WindSensor {
+        double wind_speed;
+        double wind_direction;
+    };
+
     struct Gps {
         uint64_t time_utc_usec;
         int fix_type;
@@ -129,12 +135,15 @@ struct HILData {
     int id=-1;
     bool baro_updated{false};
     bool diff_press_updated{false};
+    bool windsensor_updated{false};
     bool mag_updated{false};
     bool imu_updated{false};
     double temperature;
     double pressure_alt;
     double abs_pressure;
     double diff_pressure;
+    double wind_direction;
+    double wind_speed;
     Eigen::Vector3d mag_b;
     Eigen::Vector3d accel_b;
     Eigen::Vector3d gyro_b;
@@ -158,6 +167,7 @@ class MavlinkInterface {
     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);
+    void UpdateWindSensor(const SensorData::WindSensor &data, const int id = 0);
     void UpdateIMU(const SensorData::Imu &data, const int id = 0);
     void UpdateMag(const SensorData::Magnetometer &data, const int id = 0);
     Eigen::VectorXd GetActuatorControls();

models/windsensor/model.config

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

models/windsensor/windsensor.sdf

@@ -0,0 +1,30 @@
+<?xml version="1.0" ?>
+<sdf version="1.6">
+  <model name="windsensor">
+    <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='windsensor' 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_windsensor_plugin' filename='libgazebo_windsensor_plugin.so'>
+          <robotNamespace/>
+        </plugin>
+      </sensor>
+    </link>
+  </model>
+</sdf>

msgs/WindSensor.proto

@@ -0,0 +1,9 @@
+syntax = "proto2";
+package sensor_msgs.msgs;
+
+message WindSensor
+{
+  required int64  time_usec             = 1;
+  required double wind_speed            = 2;
+  required double wind_direction        = 3;
+}

src/gazebo_mavlink_interface.cpp

@@ -443,6 +443,7 @@ void GazeboMavlinkInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf
   CreateSensorSubscription(&GazeboMavlinkInterface::SonarCallback, this, joints, nested_model, kDefaultSonarModelNaming);
   CreateSensorSubscription(&GazeboMavlinkInterface::GpsCallback, this, joints, nested_model, kDefaultGPSModelNaming);
   CreateSensorSubscription(&GazeboMavlinkInterface::AirspeedCallback, this, joints, nested_model, kDefaultAirspeedModelJointNaming);
+  CreateSensorSubscription(&GazeboMavlinkInterface::WindSensorCallback, this, joints, nested_model, kDefaultWindSensorModelJointNaming);
   CreateSensorSubscription(&GazeboMavlinkInterface::ImuCallback, this, joints, nested_model, kDefaultImuModelJointNaming);
   CreateSensorSubscription(&GazeboMavlinkInterface::MagnetometerCallback, this, joints, nested_model, kDefaultMagModelJointNaming);
 
@@ -1092,6 +1093,13 @@ void GazeboMavlinkInterface::AirspeedCallback(AirspeedPtr& airspeed_msg, const i
   mavlink_interface_->UpdateAirspeed(airspeed_data, id);
 }
 
+void GazeboMavlinkInterface::WindSensorCallback(WindSensorPtr& windsensor_msg, const int& id) {
+  SensorData::WindSensor windsensor_data;
+  windsensor_data.wind_speed = windsensor_msg->wind_speed();
+  windsensor_data.wind_direction = windsensor_msg->wind_direction();
+  mavlink_interface_->UpdateWindSensor(windsensor_data, id);
+}
+
 void GazeboMavlinkInterface::BarometerCallback(BarometerPtr& baro_msg) {
   SensorData::Barometer baro_data;
   baro_data.temperature = baro_msg->temperature();