This gazebo plugin simulates wind acting on a model. More...
#include <gazebo_wind_plugin.h>
Public Member Functions | |
GazeboWindPlugin () | |
virtual | ~GazeboWindPlugin () |
Protected Member Functions | |
void | Load (physics::ModelPtr _model, sdf::ElementPtr _sdf) |
Load the plugin. | |
void | OnUpdate (const common::UpdateInfo &) |
Called when the world is updated. | |
Private Member Functions | |
ignition::math::Vector3d | BilinearInterpolation (double *position, ignition::math::Vector3d *values, double *points) const |
Bilinear interpolation. | |
void | CreatePubsAndSubs () |
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required. | |
ignition::math::Vector3d | LinearInterpolation (double position, ignition::math::Vector3d *values, double *points) const |
Functions for trilinear interpolation of wind field at aircraft position. | |
void | ReadCustomWindField (std::string &custom_wind_field_path) |
Reads wind data from a text file and saves it. | |
ignition::math::Vector3d | TrilinearInterpolation (ignition::math::Vector3d link_position, ignition::math::Vector3d *values, double *points) const |
Trilinear interpolation. | |
Private Attributes | |
std::vector< float > | bottom_z_ |
std::string | frame_id_ |
physics::LinkPtr | link_ |
std::string | link_name_ |
float | min_x_ |
float | min_y_ |
physics::ModelPtr | model_ |
int | n_x_ |
int | n_y_ |
std::string | namespace_ |
gazebo::transport::NodePtr | node_handle_ |
bool | pubs_and_subs_created_ |
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from be called on every OnUpdate(). | |
float | res_x_ |
float | res_y_ |
std::vector< float > | top_z_ |
std::vector< float > | u_ |
event::ConnectionPtr | update_connection_ |
Pointer to the update event connection. | |
bool | use_custom_static_wind_field_ |
Variables for custom wind field generation. | |
std::vector< float > | v_ |
std::vector< float > | vertical_spacing_factors_ |
std::vector< float > | w_ |
ignition::math::Vector3d | wind_direction_ |
double | wind_force_mean_ |
gazebo::transport::PublisherPtr | wind_force_pub_ |
std::string | wind_force_pub_topic_ |
double | wind_force_variance_ |
ignition::math::Vector3d | wind_gust_direction_ |
common::Time | wind_gust_end_ |
double | wind_gust_force_mean_ |
double | wind_gust_force_variance_ |
common::Time | wind_gust_start_ |
double | wind_speed_mean_ |
gz_mav_msgs::WindSpeed | wind_speed_msg_ |
Gazebo message for sending wind speed data. | |
gazebo::transport::PublisherPtr | wind_speed_pub_ |
std::string | wind_speed_pub_topic_ |
double | wind_speed_variance_ |
physics::WorldPtr | world_ |
gz_geometry_msgs::WrenchStamped | wrench_stamped_msg_ |
Gazebo message for sending wind data. | |
ignition::math::Vector3d | xyz_offset_ |
This gazebo plugin simulates wind acting on a model.
This plugin publishes on a Gazebo topic and instructs the ROS interface plugin to forward the message onto ROS.
Definition at line 67 of file gazebo_wind_plugin.h.
gazebo::GazeboWindPlugin::GazeboWindPlugin | ( | ) | [inline] |
Definition at line 69 of file gazebo_wind_plugin.h.
gazebo::GazeboWindPlugin::~GazeboWindPlugin | ( | ) | [virtual] |
Definition at line 31 of file gazebo_wind_plugin.cpp.
ignition::math::Vector3d gazebo::GazeboWindPlugin::BilinearInterpolation | ( | double * | position, |
ignition::math::Vector3d * | values, | ||
double * | points | ||
) | const [private] |
Bilinear interpolation.
[in] | position | Pointer to an array of size 2 containing the x- and y-coordinates of the target point. values Pointer to an array of size 4 containing the wind values of the four points to interpolate from (8, 9, 10 and 11). points Pointer to an array of size 14 containing the z-coordinate of the eight points to interpolate from, the x-coordinate of the four intermediate points (8, 9, 10 and 11), and the y-coordinate of the last two intermediate points (12 and 13). |
Definition at line 416 of file gazebo_wind_plugin.cpp.
void gazebo::GazeboWindPlugin::CreatePubsAndSubs | ( | ) | [private] |
Creates all required publishers and subscribers, incl. routing of messages to/from ROS if required.
Call this once the first time OnUpdate() is called (can't be called from Load() because there is no guarantee GazeboRosInterfacePlugin has has loaded and listening to ConnectGazeboToRosTopic and ConnectRosToGazeboTopic messages).
Definition at line 300 of file gazebo_wind_plugin.cpp.
ignition::math::Vector3d gazebo::GazeboWindPlugin::LinearInterpolation | ( | double | position, |
ignition::math::Vector3d * | values, | ||
double * | points | ||
) | const [private] |
Functions for trilinear interpolation of wind field at aircraft position.
Linear interpolation
[in] | position | y-coordinate of the target point. values Pointer to an array of size 2 containing the wind values of the two points to interpolate from (12 and 13). points Pointer to an array of size 2 containing the y-coordinate of the two points to interpolate from. |
Definition at line 409 of file gazebo_wind_plugin.cpp.
void gazebo::GazeboWindPlugin::Load | ( | physics::ModelPtr | _model, |
sdf::ElementPtr | _sdf | ||
) | [protected] |
Load the plugin.
[in] | _model | Pointer to the model that loaded this plugin. |
[in] | _sdf | SDF element that describes the plugin. |
Definition at line 35 of file gazebo_wind_plugin.cpp.
void gazebo::GazeboWindPlugin::OnUpdate | ( | const common::UpdateInfo & | _info | ) | [protected] |
Called when the world is updated.
[in] | _info | Update timing information. |
Definition at line 126 of file gazebo_wind_plugin.cpp.
void gazebo::GazeboWindPlugin::ReadCustomWindField | ( | std::string & | custom_wind_field_path | ) | [private] |
Reads wind data from a text file and saves it.
[in] | custom_wind_field_path | Path to the wind field from ~/.ros. |
Definition at line 340 of file gazebo_wind_plugin.cpp.
ignition::math::Vector3d gazebo::GazeboWindPlugin::TrilinearInterpolation | ( | ignition::math::Vector3d | link_position, |
ignition::math::Vector3d * | values, | ||
double * | points | ||
) | const [private] |
Trilinear interpolation.
[in] | link_position | Vector3 containing the x, y and z-coordinates of the target point. values Pointer to an array of size 8 containing the wind values of the eight points to interpolate from (0, 1, 2, 3, 4, 5, 6 and 7). points Pointer to an array of size 14 containing the z-coordinate of the eight points to interpolate from, the x-coordinate of the four intermediate points (8, 9, 10 and 11), and the y-coordinate of the last two intermediate points (12 and 13). |
Definition at line 427 of file gazebo_wind_plugin.cpp.
std::vector<float> gazebo::GazeboWindPlugin::bottom_z_ [private] |
Definition at line 150 of file gazebo_wind_plugin.h.
Definition at line 122 of file gazebo_wind_plugin.h.
physics::LinkPtr gazebo::GazeboWindPlugin::link_ [private] |
Definition at line 118 of file gazebo_wind_plugin.h.
Definition at line 123 of file gazebo_wind_plugin.h.
float gazebo::GazeboWindPlugin::min_x_ [private] |
Definition at line 143 of file gazebo_wind_plugin.h.
float gazebo::GazeboWindPlugin::min_y_ [private] |
Definition at line 144 of file gazebo_wind_plugin.h.
physics::ModelPtr gazebo::GazeboWindPlugin::model_ [private] |
Definition at line 117 of file gazebo_wind_plugin.h.
int gazebo::GazeboWindPlugin::n_x_ [private] |
Definition at line 145 of file gazebo_wind_plugin.h.
int gazebo::GazeboWindPlugin::n_y_ [private] |
Definition at line 146 of file gazebo_wind_plugin.h.
Definition at line 120 of file gazebo_wind_plugin.h.
gazebo::transport::NodePtr gazebo::GazeboWindPlugin::node_handle_ [private] |
Definition at line 195 of file gazebo_wind_plugin.h.
bool gazebo::GazeboWindPlugin::pubs_and_subs_created_ [private] |
Flag that is set to true once CreatePubsAndSubs() is called, used to prevent CreatePubsAndSubs() from be called on every OnUpdate().
Definition at line 105 of file gazebo_wind_plugin.h.
float gazebo::GazeboWindPlugin::res_x_ [private] |
Definition at line 147 of file gazebo_wind_plugin.h.
float gazebo::GazeboWindPlugin::res_y_ [private] |
Definition at line 148 of file gazebo_wind_plugin.h.
std::vector<float> gazebo::GazeboWindPlugin::top_z_ [private] |
Definition at line 151 of file gazebo_wind_plugin.h.
std::vector<float> gazebo::GazeboWindPlugin::u_ [private] |
Definition at line 152 of file gazebo_wind_plugin.h.
Pointer to the update event connection.
Definition at line 114 of file gazebo_wind_plugin.h.
bool gazebo::GazeboWindPlugin::use_custom_static_wind_field_ [private] |
Variables for custom wind field generation.
Definition at line 142 of file gazebo_wind_plugin.h.
std::vector<float> gazebo::GazeboWindPlugin::v_ [private] |
Definition at line 153 of file gazebo_wind_plugin.h.
std::vector<float> gazebo::GazeboWindPlugin::vertical_spacing_factors_ [private] |
Definition at line 149 of file gazebo_wind_plugin.h.
std::vector<float> gazebo::GazeboWindPlugin::w_ [private] |
Definition at line 154 of file gazebo_wind_plugin.h.
ignition::math::Vector3d gazebo::GazeboWindPlugin::wind_direction_ [private] |
Definition at line 135 of file gazebo_wind_plugin.h.
double gazebo::GazeboWindPlugin::wind_force_mean_ [private] |
Definition at line 127 of file gazebo_wind_plugin.h.
gazebo::transport::PublisherPtr gazebo::GazeboWindPlugin::wind_force_pub_ [private] |
Definition at line 192 of file gazebo_wind_plugin.h.
Definition at line 124 of file gazebo_wind_plugin.h.
double gazebo::GazeboWindPlugin::wind_force_variance_ [private] |
Definition at line 128 of file gazebo_wind_plugin.h.
ignition::math::Vector3d gazebo::GazeboWindPlugin::wind_gust_direction_ [private] |
Definition at line 136 of file gazebo_wind_plugin.h.
common::Time gazebo::GazeboWindPlugin::wind_gust_end_ [private] |
Definition at line 138 of file gazebo_wind_plugin.h.
double gazebo::GazeboWindPlugin::wind_gust_force_mean_ [private] |
Definition at line 129 of file gazebo_wind_plugin.h.
double gazebo::GazeboWindPlugin::wind_gust_force_variance_ [private] |
Definition at line 130 of file gazebo_wind_plugin.h.
common::Time gazebo::GazeboWindPlugin::wind_gust_start_ [private] |
Definition at line 139 of file gazebo_wind_plugin.h.
double gazebo::GazeboWindPlugin::wind_speed_mean_ [private] |
Definition at line 131 of file gazebo_wind_plugin.h.
gz_mav_msgs::WindSpeed gazebo::GazeboWindPlugin::wind_speed_msg_ [private] |
Gazebo message for sending wind speed data.
This is defined at the class scope so that it is re-created everytime a wind speed message needs to be sent, increasing performance.
Definition at line 205 of file gazebo_wind_plugin.h.
gazebo::transport::PublisherPtr gazebo::GazeboWindPlugin::wind_speed_pub_ [private] |
Definition at line 193 of file gazebo_wind_plugin.h.
Definition at line 125 of file gazebo_wind_plugin.h.
double gazebo::GazeboWindPlugin::wind_speed_variance_ [private] |
Definition at line 132 of file gazebo_wind_plugin.h.
physics::WorldPtr gazebo::GazeboWindPlugin::world_ [private] |
Definition at line 116 of file gazebo_wind_plugin.h.
gz_geometry_msgs::WrenchStamped gazebo::GazeboWindPlugin::wrench_stamped_msg_ [private] |
Gazebo message for sending wind data.
This is defined at the class scope so that it is re-created everytime a wind message needs to be sent, increasing performance.
Definition at line 200 of file gazebo_wind_plugin.h.
ignition::math::Vector3d gazebo::GazeboWindPlugin::xyz_offset_ [private] |
Definition at line 134 of file gazebo_wind_plugin.h.