Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #ifndef ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H
00024 #define ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H
00025
00026 #include <string>
00027
00028 #include <gazebo/common/common.hh>
00029 #include <gazebo/common/Plugin.hh>
00030 #include <gazebo/gazebo.hh>
00031 #include <gazebo/physics/physics.hh>
00032
00033 #include <mav_msgs/default_topics.h>
00034
00035 #include "rotors_gazebo_plugins/common.h"
00036
00037 #include "WindSpeed.pb.h"
00038 #include "WrenchStamped.pb.h"
00039
00040 namespace gazebo {
00041
00042 static const std::string kDefaultFrameId = "world";
00043 static const std::string kDefaultLinkName = "base_link";
00044 static const std::string kDefaultWindSpeedPubTopic = "wind_speed";
00045
00046 static constexpr double kDefaultWindForceMean = 0.0;
00047 static constexpr double kDefaultWindForceVariance = 0.0;
00048 static constexpr double kDefaultWindGustForceMean = 0.0;
00049 static constexpr double kDefaultWindGustForceVariance = 0.0;
00050
00051 static constexpr double kDefaultWindGustStart = 10.0;
00052 static constexpr double kDefaultWindGustDuration = 0.0;
00053
00054 static constexpr double kDefaultWindSpeedMean = 0.0;
00055 static constexpr double kDefaultWindSpeedVariance = 0.0;
00056
00057 static const ignition::math::Vector3d kDefaultWindDirection = ignition::math::Vector3d (1, 0, 0);
00058 static const ignition::math::Vector3d kDefaultWindGustDirection = ignition::math::Vector3d (0, 1, 0);
00059
00060 static constexpr bool kDefaultUseCustomStaticWindField = false;
00061
00062
00063
00067 class GazeboWindPlugin : public ModelPlugin {
00068 public:
00069 GazeboWindPlugin()
00070 : ModelPlugin(),
00071 namespace_(kDefaultNamespace),
00072 wind_force_pub_topic_(mav_msgs::default_topics::EXTERNAL_FORCE),
00073 wind_speed_pub_topic_(mav_msgs::default_topics::WIND_SPEED),
00074 wind_force_mean_(kDefaultWindForceMean),
00075 wind_force_variance_(kDefaultWindForceVariance),
00076 wind_gust_force_mean_(kDefaultWindGustForceMean),
00077 wind_gust_force_variance_(kDefaultWindGustForceVariance),
00078 wind_speed_mean_(kDefaultWindSpeedMean),
00079 wind_speed_variance_(kDefaultWindSpeedVariance),
00080 wind_direction_(kDefaultWindDirection),
00081 wind_gust_direction_(kDefaultWindGustDirection),
00082 use_custom_static_wind_field_(kDefaultUseCustomStaticWindField),
00083 frame_id_(kDefaultFrameId),
00084 link_name_(kDefaultLinkName),
00085 node_handle_(nullptr),
00086 pubs_and_subs_created_(false) {}
00087
00088 virtual ~GazeboWindPlugin();
00089
00090 protected:
00091
00095 void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00096
00099 void OnUpdate(const common::UpdateInfo& );
00100
00101 private:
00102
00105 bool pubs_and_subs_created_;
00106
00111 void CreatePubsAndSubs();
00112
00114 event::ConnectionPtr update_connection_;
00115
00116 physics::WorldPtr world_;
00117 physics::ModelPtr model_;
00118 physics::LinkPtr link_;
00119
00120 std::string namespace_;
00121
00122 std::string frame_id_;
00123 std::string link_name_;
00124 std::string wind_force_pub_topic_;
00125 std::string wind_speed_pub_topic_;
00126
00127 double wind_force_mean_;
00128 double wind_force_variance_;
00129 double wind_gust_force_mean_;
00130 double wind_gust_force_variance_;
00131 double wind_speed_mean_;
00132 double wind_speed_variance_;
00133
00134 ignition::math::Vector3d xyz_offset_;
00135 ignition::math::Vector3d wind_direction_;
00136 ignition::math::Vector3d wind_gust_direction_;
00137
00138 common::Time wind_gust_end_;
00139 common::Time wind_gust_start_;
00140
00142 bool use_custom_static_wind_field_;
00143 float min_x_;
00144 float min_y_;
00145 int n_x_;
00146 int n_y_;
00147 float res_x_;
00148 float res_y_;
00149 std::vector<float> vertical_spacing_factors_;
00150 std::vector<float> bottom_z_;
00151 std::vector<float> top_z_;
00152 std::vector<float> u_;
00153 std::vector<float> v_;
00154 std::vector<float> w_;
00155
00158 void ReadCustomWindField(std::string& custom_wind_field_path);
00159
00161
00168 ignition::math::Vector3d LinearInterpolation(double position, ignition::math::Vector3d * values, double* points) const;
00169
00179 ignition::math::Vector3d BilinearInterpolation(double* position, ignition::math::Vector3d * values, double* points) const;
00180
00190 ignition::math::Vector3d TrilinearInterpolation(ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const;
00191
00192 gazebo::transport::PublisherPtr wind_force_pub_;
00193 gazebo::transport::PublisherPtr wind_speed_pub_;
00194
00195 gazebo::transport::NodePtr node_handle_;
00196
00200 gz_geometry_msgs::WrenchStamped wrench_stamped_msg_;
00201
00205 gz_mav_msgs::WindSpeed wind_speed_msg_;
00206 };
00207 }
00208
00209 #endif // ROTORS_GAZEBO_PLUGINS_GAZEBO_WIND_PLUGIN_H
rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43