// Called by the world update start event
void GazeboWindPlugin::OnUpdate(const common::UpdateInfo& _info) {
// Get the current simulation time.
common::Time now = world_->GetSimTime();
// Constant wind
double wind_strength = wind_force_mean_;
math::Vector3 wind = wind_strength * wind_direction_;
// Apply a force from the constant wind to the link
this->link_->AddForceAtRelativePosition(wind, xyz_offset_);
math::Vector3 wind_gust(0, 0, 0);
// Wind Gusts
if (now >= wind_gust_start_ && now < wind_gust_end_) {
double wind_gust_strength = wind_gust_force_mean_;
wind_gust = wind_gust_strength * wind_gust_direction_;
// Apply a force from the wind gust to the link
this->link_->AddForceAtRelativePosition(wind_gust, xyz_offset_);
}
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.frame_id = frame_id_;
wrench_msg.header.stamp.sec = now.sec;
wrench_msg.header.stamp.nsec = now.nsec;
wrench_msg.wrench.force.x = wind.x + wind_gust.x;
wrench_msg.wrench.force.y = wind.y + wind_gust.y;
wrench_msg.wrench.force.z = wind.z + wind_gust.z;
wrench_msg.wrench.torque.x = 0;
wrench_msg.wrench.torque.y = 0;
wrench_msg.wrench.torque.z = 0;
wind_pub_.publish(wrench_msg);
}
// This gets called by the world update start event.
void GazeboWindPlugin::OnUpdate(const common::UpdateInfo& _info) {
// Get the current simulation time.
common::Time now = world_->GetSimTime();
// Establish Random Number Generator
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
std::default_random_engine generator (seed);
std::uniform_real_distribution<double> direction_distribution(-1,1);
std::normal_distribution<double> force_distribution(wind_force_mean_,sqrt(wind_force_variance_));
std::normal_distribution<double> gust_force_distribution(wind_gust_force_mean_,sqrt(wind_gust_force_variance_));
std::normal_distribution<double> wind_change_distribution(1000,500);
// Calculate the wind force
if (wind_change_delay==wind_change_value)
{
wind_strength = force_distribution(generator);
wind_x = direction_distribution(generator);
wind_y = direction_distribution(generator);
wind_z = direction_distribution(generator);
wind_change_value = ceil(abs(wind_change_distribution(generator)));
wind_change_delay=0;
}
else
{
wind_change_delay++;
}
//Vary the wind direction
wind_direction={wind_x, wind_y, wind_z};
wind_direction.Normalize();
math::Vector3 wind = wind_strength * wind_direction;
// Apply a force from the wind to the link.
link_->AddForceAtRelativePosition(wind, xyz_offset_);
//Wind Gust
math::Vector3 wind_gust(0, 0, 0);
// Calculate the wind gust force.
if (now >= wind_gust_start_ && now < wind_gust_end_) {
double wind_gust_strength = gust_force_distribution(generator);
wind_gust = wind_gust_strength * wind_gust_direction_;
// Apply a force from the wind gust to the link.
link_->AddForceAtRelativePosition(wind_gust, xyz_offset_);
}
geometry_msgs::Vector3 wind_msg;
wind_msg.x = wind.x + wind_gust.x;
wind_msg.y = wind.y + wind_gust.y;
wind_msg.z = wind.z + wind_gust.z;
wind_pub_.publish(wind_msg);
}
// This gets called by the world update start event.
void GazeboWindPlugin::OnUpdate(const common::UpdateInfo& _info) {
if (kPrintOnUpdates) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
if (!pubs_and_subs_created_) {
CreatePubsAndSubs();
pubs_and_subs_created_ = true;
}
// Get the current simulation time.
common::Time now = world_->GetSimTime();
math::Vector3 wind_velocity(0.0, 0.0, 0.0);
// Choose user-specified method for calculating wind velocity.
if (!use_custom_static_wind_field_) {
// Calculate the wind force.
double wind_strength = wind_force_mean_;
math::Vector3 wind = wind_strength * wind_direction_;
// Apply a force from the constant wind to the link.
link_->AddForceAtRelativePosition(wind, xyz_offset_);
math::Vector3 wind_gust(0.0, 0.0, 0.0);
// Calculate the wind gust force.
if (now >= wind_gust_start_ && now < wind_gust_end_) {
double wind_gust_strength = wind_gust_force_mean_;
wind_gust = wind_gust_strength * wind_gust_direction_;
// Apply a force from the wind gust to the link.
link_->AddForceAtRelativePosition(wind_gust, xyz_offset_);
}
wrench_stamped_msg_.mutable_header()->set_frame_id(frame_id_);
wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_x(wind.x +
wind_gust.x);
wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_y(wind.y +
wind_gust.y);
wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_z(wind.z +
wind_gust.z);
// No torque due to wind, set x,y and z to 0.
wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_x(0);
wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_y(0);
wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_z(0);
wind_force_pub_->Publish(wrench_stamped_msg_);
// Calculate the wind speed.
wind_velocity = wind_speed_mean_ * wind_direction_;
} else {
// Get the current position of the aircraft in world coordinates.
math::Vector3 link_position = link_->GetWorldPose().pos;
// Calculate the x, y index of the grid points with x, y-coordinate
// just smaller than or equal to aircraft x, y position.
std::size_t x_inf = floor((link_position.x - min_x_) / res_x_);
std::size_t y_inf = floor((link_position.y - min_y_) / res_y_);
// In case aircraft is on one of the boundary surfaces at max_x or max_y,
// decrease x_inf, y_inf by one to have x_sup, y_sup on max_x, max_y.
if (x_inf == n_x_ - 1u) {
x_inf = n_x_ - 2u;
}
if (y_inf == n_y_ - 1u) {
y_inf = n_y_ - 2u;
}
// Calculate the x, y index of the grid points with x, y-coordinate just
// greater than the aircraft x, y position.
std::size_t x_sup = x_inf + 1u;
std::size_t y_sup = y_inf + 1u;
// Save in an array the x, y index of each of the eight grid points
// enclosing the aircraft.
constexpr unsigned int n_vertices = 8;
std::size_t idx_x[n_vertices] = {x_inf, x_inf, x_sup, x_sup, x_inf, x_inf, x_sup, x_sup};
std::size_t idx_y[n_vertices] = {y_inf, y_inf, y_inf, y_inf, y_sup, y_sup, y_sup, y_sup};
// Find the vertical factor of the aircraft in each of the four surrounding
// grid columns, and their minimal/maximal value.
constexpr unsigned int n_columns = 4;
float vertical_factors_columns[n_columns];
for (std::size_t i = 0u; i < n_columns; ++i) {
vertical_factors_columns[i] = (
link_position.z - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]) /
(top_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_] - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]);
}
// Find maximal and minimal value amongst vertical factors.
float vertical_factors_min = std::min(std::min(std::min(
vertical_factors_columns[0], vertical_factors_columns[1]),
vertical_factors_columns[2]), vertical_factors_columns[3]);
float vertical_factors_max = std::max(std::max(std::max(
vertical_factors_columns[0], vertical_factors_columns[1]),
vertical_factors_columns[2]), vertical_factors_columns[3]);
// Check if aircraft is out of wind field or not, and act accordingly.
if (x_inf >= 0u && y_inf >= 0u && vertical_factors_max >= 0u &&
x_sup <= (n_x_ - 1u) && y_sup <= (n_y_ - 1u) && vertical_factors_min <= 1u) {
// Find indices in z-direction for each of the vertices. If link is not
// within the range of one of the columns, set to lowest or highest two.
std::size_t idx_z[n_vertices] = {0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u};
for (std::size_t i = 0u; i < n_columns; ++i) {
if (vertical_factors_columns[i] < 0u) {
// Link z-position below lowest grid point of that column.
idx_z[2u * i + 1u] = 1u;
} else if (vertical_factors_columns[i] >= 1u) {
// Link z-position above highest grid point of that column.
idx_z[2u * i] = vertical_spacing_factors_.size() - 2u;
} else {
// Link z-position between two grid points in that column.
for (std::size_t j = 0u; j < vertical_spacing_factors_.size() - 1u; ++j) {
if (vertical_spacing_factors_[j] <= vertical_factors_columns[i] &&
vertical_spacing_factors_[j + 1u] > vertical_factors_columns[i]) {
idx_z[2u * i] = j;
idx_z[2u * i + 1u] = j + 1u;
break;
}
}
}
}
// Extract the wind velocities corresponding to each vertex.
math::Vector3 wind_at_vertices[n_vertices];
for (std::size_t i = 0u; i < n_vertices; ++i) {
wind_at_vertices[i].x = u_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
wind_at_vertices[i].y = v_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
wind_at_vertices[i].z = w_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
}
// Extract the relevant coordinate of every point needed for trilinear
// interpolation (first z-direction, then x-direction, then y-direction).
constexpr unsigned int n_points_interp_z = 8;
constexpr unsigned int n_points_interp_x = 4;
constexpr unsigned int n_points_interp_y = 2;
double interpolation_points[n_points_interp_x + n_points_interp_y + n_points_interp_z];
for (std::size_t i = 0u; i < n_points_interp_x + n_points_interp_y + n_points_interp_z; ++i) {
if (i < n_points_interp_z) {
interpolation_points[i] = (
top_z_[idx_x[i] + idx_y[i] * n_x_] - bottom_z_[idx_x[i] + idx_y[i] * n_x_])
* vertical_spacing_factors_[idx_z[i]] + bottom_z_[idx_x[i] + idx_y[i] * n_x_];
} else if (i >= n_points_interp_z && i < n_points_interp_x + n_points_interp_z) {
interpolation_points[i] = min_x_ + res_x_ * idx_x[2u * (i - n_points_interp_z)];
} else {
interpolation_points[i] = min_y_ + res_y_ * idx_y[4u * (i - n_points_interp_z - n_points_interp_x)];
}
}
// Interpolate wind velocity at aircraft position.
wind_velocity = TrilinearInterpolation(
link_position, wind_at_vertices, interpolation_points);
} else {
// Set the wind velocity to the default constant value specified by user.
wind_velocity = wind_speed_mean_ * wind_direction_;
}
}
wind_speed_msg_.mutable_header()->set_frame_id(frame_id_);
wind_speed_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
wind_speed_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
wind_speed_msg_.mutable_velocity()->set_x(wind_velocity.x);
wind_speed_msg_.mutable_velocity()->set_y(wind_velocity.y);
wind_speed_msg_.mutable_velocity()->set_z(wind_velocity.z);
wind_speed_pub_->Publish(wind_speed_msg_);
}
