gazebo_wind_plugin.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  * Copyright 2016 Geoffrey Hunter <gbmhunter@gmail.com>
00008  *
00009  * Licensed under the Apache License, Version 2.0 (the "License");
00010  * you may not use this file except in compliance with the License.
00011  * You may obtain a copy of the License at
00012  *
00013  *     http://www.apache.org/licenses/LICENSE-2.0
00014 
00015  * Unless required by applicable law or agreed to in writing, software
00016  * distributed under the License is distributed on an "AS IS" BASIS,
00017  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00018  * See the License for the specific language governing permissions and
00019  * limitations under the License.
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>  // This comes from the mav_comm repo
00034 
00035 #include "rotors_gazebo_plugins/common.h"
00036 
00037 #include "WindSpeed.pb.h"             // Wind speed message
00038 #include "WrenchStamped.pb.h"         // Wind force message
00039 
00040 namespace gazebo {
00041 // Default values
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& /*_info*/);
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