gazebo_wind_plugin.cpp
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 #include "rotors_gazebo_plugins/gazebo_wind_plugin.h"
00023 
00024 #include <fstream>
00025 #include <math.h>
00026 
00027 #include "ConnectGazeboToRosTopic.pb.h"
00028 
00029 namespace gazebo {
00030 
00031 GazeboWindPlugin::~GazeboWindPlugin() {
00032   
00033 }
00034 
00035 void GazeboWindPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
00036   if (kPrintOnPluginLoad) {
00037     gzdbg << __FUNCTION__ << "() called." << std::endl;
00038   }
00039 
00040   // Store the pointer to the model.
00041   model_ = _model;
00042   world_ = model_->GetWorld();
00043 
00044   double wind_gust_start = kDefaultWindGustStart;
00045   double wind_gust_duration = kDefaultWindGustDuration;
00046 
00047   //==============================================//
00048   //========== READ IN PARAMS FROM SDF ===========//
00049   //==============================================//
00050 
00051   if (_sdf->HasElement("robotNamespace"))
00052     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00053   else
00054     gzerr << "[gazebo_wind_plugin] Please specify a robotNamespace.\n";
00055 
00056   // Create Gazebo Node.
00057   node_handle_ = gazebo::transport::NodePtr(new transport::Node());
00058 
00059   // Initialise with default namespace (typically /gazebo/default/).
00060   node_handle_->Init();
00061 
00062   if (_sdf->HasElement("xyzOffset"))
00063     xyz_offset_ = _sdf->GetElement("xyzOffset")->Get<ignition::math::Vector3d >();
00064   else
00065     gzerr << "[gazebo_wind_plugin] Please specify a xyzOffset.\n";
00066 
00067   getSdfParam<std::string>(_sdf, "windForcePubTopic", wind_force_pub_topic_,
00068                            wind_force_pub_topic_);
00069   getSdfParam<std::string>(_sdf, "windSpeedPubTopic", wind_speed_pub_topic_,
00070                            wind_speed_pub_topic_);
00071   getSdfParam<std::string>(_sdf, "frameId", frame_id_, frame_id_);
00072   getSdfParam<std::string>(_sdf, "linkName", link_name_, link_name_);
00073   // Get the wind speed params from SDF.
00074   getSdfParam<double>(_sdf, "windSpeedMean", wind_speed_mean_,
00075                       wind_speed_mean_);
00076   getSdfParam<double>(_sdf, "windSpeedVariance", wind_speed_variance_,
00077                       wind_speed_variance_);
00078   getSdfParam<ignition::math::Vector3d >(_sdf, "windDirection", wind_direction_,
00079                       wind_direction_);
00080   // Check if a custom static wind field should be used.
00081   getSdfParam<bool>(_sdf, "useCustomStaticWindField", use_custom_static_wind_field_,
00082                       use_custom_static_wind_field_);
00083   if (!use_custom_static_wind_field_) {
00084     gzdbg << "[gazebo_wind_plugin] Using user-defined constant wind field and gusts.\n";
00085     // Get the wind params from SDF.
00086     getSdfParam<double>(_sdf, "windForceMean", wind_force_mean_,
00087                         wind_force_mean_);
00088     getSdfParam<double>(_sdf, "windForceVariance", wind_force_variance_,
00089                         wind_force_variance_);
00090     // Get the wind gust params from SDF.
00091     getSdfParam<double>(_sdf, "windGustStart", wind_gust_start, wind_gust_start);
00092     getSdfParam<double>(_sdf, "windGustDuration", wind_gust_duration,
00093                         wind_gust_duration);
00094     getSdfParam<double>(_sdf, "windGustForceMean", wind_gust_force_mean_,
00095                         wind_gust_force_mean_);
00096     getSdfParam<double>(_sdf, "windGustForceVariance", wind_gust_force_variance_,
00097                         wind_gust_force_variance_);
00098     getSdfParam<ignition::math::Vector3d >(_sdf, "windGustDirection", wind_gust_direction_,
00099                         wind_gust_direction_);
00100 
00101     wind_direction_.Normalize();
00102     wind_gust_direction_.Normalize();
00103     wind_gust_start_ = common::Time(wind_gust_start);
00104     wind_gust_end_ = common::Time(wind_gust_start + wind_gust_duration);
00105   } else {
00106     gzdbg << "[gazebo_wind_plugin] Using custom wind field from text file.\n";
00107     // Get the wind field text file path, read it and save data.
00108     std::string custom_wind_field_path;
00109     getSdfParam<std::string>(_sdf, "customWindFieldPath", custom_wind_field_path,
00110                         custom_wind_field_path);
00111     ReadCustomWindField(custom_wind_field_path);
00112   }
00113 
00114   link_ = model_->GetLink(link_name_);
00115   if (link_ == NULL)
00116     gzthrow("[gazebo_wind_plugin] Couldn't find specified link \"" << link_name_
00117                                                                    << "\".");
00118 
00119   // Listen to the update event. This event is broadcast every
00120   // simulation iteration.
00121   update_connection_ = event::Events::ConnectWorldUpdateBegin(
00122       boost::bind(&GazeboWindPlugin::OnUpdate, this, _1));
00123 }
00124 
00125 // This gets called by the world update start event.
00126 void GazeboWindPlugin::OnUpdate(const common::UpdateInfo& _info) {
00127   if (kPrintOnUpdates) {
00128     gzdbg << __FUNCTION__ << "() called." << std::endl;
00129   }
00130 
00131   if (!pubs_and_subs_created_) {
00132     CreatePubsAndSubs();
00133     pubs_and_subs_created_ = true;
00134   }
00135 
00136   // Get the current simulation time.
00137   common::Time now = world_->SimTime();
00138   
00139   ignition::math::Vector3d wind_velocity(0.0, 0.0, 0.0);
00140 
00141   // Choose user-specified method for calculating wind velocity.
00142   if (!use_custom_static_wind_field_) {
00143     // Calculate the wind force.
00144     double wind_strength = wind_force_mean_;
00145     ignition::math::Vector3d wind = wind_strength * wind_direction_;
00146     // Apply a force from the constant wind to the link.
00147     link_->AddForceAtRelativePosition(wind, xyz_offset_);
00148 
00149     ignition::math::Vector3d wind_gust(0.0, 0.0, 0.0);
00150     // Calculate the wind gust force.
00151     if (now >= wind_gust_start_ && now < wind_gust_end_) {
00152       double wind_gust_strength = wind_gust_force_mean_;
00153       wind_gust = wind_gust_strength * wind_gust_direction_;
00154       // Apply a force from the wind gust to the link.
00155       link_->AddForceAtRelativePosition(wind_gust, xyz_offset_);
00156     }
00157 
00158     wrench_stamped_msg_.mutable_header()->set_frame_id(frame_id_);
00159     wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
00160     wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
00161 
00162     wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_x(wind.X() +
00163                                                                  wind_gust.X());
00164     wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_y(wind.Y() +
00165                                                                  wind_gust.Y());
00166     wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_z(wind.Z() +
00167                                                                  wind_gust.Z());
00168 
00169     // No torque due to wind, set x,y and z to 0.
00170     wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_x(0);
00171     wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_y(0);
00172     wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_z(0);
00173 
00174     wind_force_pub_->Publish(wrench_stamped_msg_);
00175 
00176     // Calculate the wind speed.
00177     wind_velocity = wind_speed_mean_ * wind_direction_;
00178   } else {
00179     // Get the current position of the aircraft in world coordinates.
00180     ignition::math::Vector3d link_position = link_->WorldPose().Pos();
00181 
00182     // Calculate the x, y index of the grid points with x, y-coordinate 
00183     // just smaller than or equal to aircraft x, y position.
00184     std::size_t x_inf = floor((link_position.X() - min_x_) / res_x_);
00185     std::size_t y_inf = floor((link_position.Y() - min_y_) / res_y_);
00186 
00187     // In case aircraft is on one of the boundary surfaces at max_x or max_y,
00188     // decrease x_inf, y_inf by one to have x_sup, y_sup on max_x, max_y.
00189     if (x_inf == n_x_ - 1u) {
00190       x_inf = n_x_ - 2u;
00191     }
00192     if (y_inf == n_y_ - 1u) {
00193       y_inf = n_y_ - 2u;
00194     }
00195 
00196     // Calculate the x, y index of the grid points with x, y-coordinate just
00197     // greater than the aircraft x, y position. 
00198     std::size_t x_sup = x_inf + 1u;
00199     std::size_t y_sup = y_inf + 1u;
00200 
00201     // Save in an array the x, y index of each of the eight grid points 
00202     // enclosing the aircraft.
00203     constexpr unsigned int n_vertices = 8;
00204     std::size_t idx_x[n_vertices] = {x_inf, x_inf, x_sup, x_sup, x_inf, x_inf, x_sup, x_sup};
00205     std::size_t idx_y[n_vertices] = {y_inf, y_inf, y_inf, y_inf, y_sup, y_sup, y_sup, y_sup};
00206 
00207     // Find the vertical factor of the aircraft in each of the four surrounding 
00208     // grid columns, and their minimal/maximal value.
00209     constexpr unsigned int n_columns = 4;
00210     float vertical_factors_columns[n_columns];
00211     for (std::size_t i = 0u; i < n_columns; ++i) {
00212       vertical_factors_columns[i] = (
00213         link_position.Z() - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]) /
00214         (top_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_] - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]);
00215     }
00216     
00217     // Find maximal and minimal value amongst vertical factors.
00218     float vertical_factors_min = std::min(std::min(std::min(
00219       vertical_factors_columns[0], vertical_factors_columns[1]),
00220       vertical_factors_columns[2]), vertical_factors_columns[3]);
00221     float vertical_factors_max = std::max(std::max(std::max(
00222       vertical_factors_columns[0], vertical_factors_columns[1]),
00223       vertical_factors_columns[2]), vertical_factors_columns[3]);
00224 
00225     // Check if aircraft is out of wind field or not, and act accordingly.
00226     if (x_inf >= 0u && y_inf >= 0u && vertical_factors_max >= 0u && 
00227         x_sup <= (n_x_ - 1u) && y_sup <= (n_y_ - 1u) && vertical_factors_min <= 1u) {
00228       // Find indices in z-direction for each of the vertices. If link is not 
00229       // within the range of one of the columns, set to lowest or highest two.
00230       std::size_t idx_z[n_vertices] = {0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
00231                               0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
00232                               0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
00233                               0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u};
00234       for (std::size_t i = 0u; i < n_columns; ++i) {
00235         if (vertical_factors_columns[i] < 0u) {
00236           // Link z-position below lowest grid point of that column.
00237           idx_z[2u * i + 1u] = 1u;
00238         } else if (vertical_factors_columns[i] >= 1u) {
00239           // Link z-position above highest grid point of that column.
00240           idx_z[2u * i] = vertical_spacing_factors_.size() - 2u;
00241         } else {
00242           // Link z-position between two grid points in that column.
00243           for (std::size_t j = 0u; j < vertical_spacing_factors_.size() - 1u; ++j) {
00244             if (vertical_spacing_factors_[j] <= vertical_factors_columns[i] && 
00245                 vertical_spacing_factors_[j + 1u] > vertical_factors_columns[i]) {
00246               idx_z[2u * i] = j;
00247               idx_z[2u * i + 1u] = j + 1u;
00248               break;
00249             }
00250           }
00251         }
00252       }
00253 
00254       // Extract the wind velocities corresponding to each vertex.
00255       ignition::math::Vector3d wind_at_vertices[n_vertices];
00256       for (std::size_t i = 0u; i < n_vertices; ++i) {
00257         wind_at_vertices[i].X() = u_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
00258         wind_at_vertices[i].Y() = v_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
00259         wind_at_vertices[i].Z() = w_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
00260       }
00261 
00262       // Extract the relevant coordinate of every point needed for trilinear 
00263       // interpolation (first z-direction, then x-direction, then y-direction).
00264       constexpr unsigned int n_points_interp_z = 8;
00265       constexpr unsigned int n_points_interp_x = 4;
00266       constexpr unsigned int n_points_interp_y = 2;
00267       double interpolation_points[n_points_interp_x + n_points_interp_y + n_points_interp_z];
00268       for (std::size_t i = 0u; i < n_points_interp_x + n_points_interp_y + n_points_interp_z; ++i) {
00269         if (i < n_points_interp_z) {
00270           interpolation_points[i] = (
00271             top_z_[idx_x[i] + idx_y[i] * n_x_] - bottom_z_[idx_x[i] + idx_y[i] * n_x_])
00272             * vertical_spacing_factors_[idx_z[i]] + bottom_z_[idx_x[i] + idx_y[i] * n_x_];
00273         } else if (i >= n_points_interp_z && i < n_points_interp_x + n_points_interp_z) {
00274           interpolation_points[i] = min_x_ + res_x_ * idx_x[2u * (i - n_points_interp_z)];
00275         } else {
00276           interpolation_points[i] = min_y_ + res_y_ * idx_y[4u * (i - n_points_interp_z - n_points_interp_x)];
00277         }
00278       }
00279 
00280       // Interpolate wind velocity at aircraft position.
00281       wind_velocity = TrilinearInterpolation(
00282         link_position, wind_at_vertices, interpolation_points);
00283     } else {
00284       // Set the wind velocity to the default constant value specified by user.
00285       wind_velocity = wind_speed_mean_ * wind_direction_;
00286     }
00287   }
00288   
00289   wind_speed_msg_.mutable_header()->set_frame_id(frame_id_);
00290   wind_speed_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
00291   wind_speed_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
00292 
00293   wind_speed_msg_.mutable_velocity()->set_x(wind_velocity.X());
00294   wind_speed_msg_.mutable_velocity()->set_y(wind_velocity.Y());
00295   wind_speed_msg_.mutable_velocity()->set_z(wind_velocity.Z());
00296 
00297   wind_speed_pub_->Publish(wind_speed_msg_);
00298 }
00299 
00300 void GazeboWindPlugin::CreatePubsAndSubs() {
00301   // Create temporary "ConnectGazeboToRosTopic" publisher and message.
00302   gazebo::transport::PublisherPtr connect_gazebo_to_ros_topic_pub =
00303       node_handle_->Advertise<gz_std_msgs::ConnectGazeboToRosTopic>(
00304           "~/" + kConnectGazeboToRosSubtopic, 1);
00305 
00306   gz_std_msgs::ConnectGazeboToRosTopic connect_gazebo_to_ros_topic_msg;
00307 
00308   // ============================================ //
00309   // ========= WRENCH STAMPED MSG SETUP ========= //
00310   // ============================================ //
00311   wind_force_pub_ = node_handle_->Advertise<gz_geometry_msgs::WrenchStamped>(
00312       "~/" + namespace_ + "/" + wind_force_pub_topic_, 1);
00313 
00314   // connect_gazebo_to_ros_topic_msg.set_gazebo_namespace(namespace_);
00315   connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00316                                                    wind_force_pub_topic_);
00317   connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00318                                                 wind_force_pub_topic_);
00319   connect_gazebo_to_ros_topic_msg.set_msgtype(
00320       gz_std_msgs::ConnectGazeboToRosTopic::WRENCH_STAMPED);
00321   connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00322                                            true);
00323 
00324   // ============================================ //
00325   // ========== WIND SPEED MSG SETUP ============ //
00326   // ============================================ //
00327   wind_speed_pub_ = node_handle_->Advertise<gz_mav_msgs::WindSpeed>(
00328       "~/" + namespace_ + "/" + wind_speed_pub_topic_, 1);
00329 
00330   connect_gazebo_to_ros_topic_msg.set_gazebo_topic("~/" + namespace_ + "/" +
00331                                                    wind_speed_pub_topic_);
00332   connect_gazebo_to_ros_topic_msg.set_ros_topic(namespace_ + "/" +
00333                                                 wind_speed_pub_topic_);
00334   connect_gazebo_to_ros_topic_msg.set_msgtype(
00335       gz_std_msgs::ConnectGazeboToRosTopic::WIND_SPEED);
00336   connect_gazebo_to_ros_topic_pub->Publish(connect_gazebo_to_ros_topic_msg,
00337                                            true);
00338 }
00339 
00340 void GazeboWindPlugin::ReadCustomWindField(std::string& custom_wind_field_path) {
00341   std::ifstream fin;
00342   fin.open(custom_wind_field_path);
00343   if (fin.is_open()) {
00344     std::string data_name;
00345     float data;
00346     // Read the line with the variable name.
00347     while (fin >> data_name) {
00348       // Save data on following line into the correct variable.
00349       if (data_name == "min_x:") {
00350         fin >> min_x_;
00351       } else if (data_name == "min_y:") {
00352         fin >> min_y_;
00353       } else if (data_name == "n_x:") {
00354         fin >> n_x_;
00355       } else if (data_name == "n_y:") {
00356         fin >> n_y_;
00357       } else if (data_name == "res_x:") {
00358         fin >> res_x_;
00359       } else if (data_name == "res_y:") {
00360         fin >> res_y_;
00361       } else if (data_name == "vertical_spacing_factors:") {
00362         while (fin >> data) {
00363           vertical_spacing_factors_.push_back(data);
00364           if (fin.peek() == '\n') break;
00365         }
00366       } else if (data_name == "bottom_z:") {
00367         while (fin >> data) {
00368           bottom_z_.push_back(data);
00369           if (fin.peek() == '\n') break;
00370         }
00371       } else if (data_name == "top_z:") {
00372         while (fin >> data) {
00373           top_z_.push_back(data);
00374           if (fin.peek() == '\n') break;
00375         }
00376       } else if (data_name == "u:") {
00377         while (fin >> data) {
00378           u_.push_back(data);
00379           if (fin.peek() == '\n') break;
00380         }
00381       } else if (data_name == "v:") {
00382         while (fin >> data) {
00383           v_.push_back(data);
00384           if (fin.peek() == '\n') break;
00385         }
00386       } else if (data_name == "w:") {
00387         while (fin >> data) {
00388           w_.push_back(data);
00389           if (fin.peek() == '\n') break;
00390         }
00391       } else {
00392         // If invalid data name, read the rest of the invalid line, 
00393         // publish a message and ignore data on next line. Then resume reading.
00394         std::string restOfLine;
00395         getline(fin, restOfLine);
00396         gzerr << " [gazebo_wind_plugin] Invalid data name '" << data_name << restOfLine <<
00397               "' in custom wind field text file. Ignoring data on next line.\n";
00398         fin.ignore(std::numeric_limits<std::streamsize>::max(), '\n');
00399       }
00400     }
00401     fin.close();
00402     gzdbg << "[gazebo_wind_plugin] Successfully read custom wind field from text file.\n";
00403   } else {
00404     gzerr << "[gazebo_wind_plugin] Could not open custom wind field text file.\n";
00405   }
00406 
00407 }
00408 
00409 ignition::math::Vector3d GazeboWindPlugin::LinearInterpolation(
00410   double position, ignition::math::Vector3d * values, double* points) const {
00411   ignition::math::Vector3d value = values[0] + (values[1] - values[0]) /
00412                         (points[1] - points[0]) * (position - points[0]);
00413   return value;
00414 }
00415 
00416 ignition::math::Vector3d GazeboWindPlugin::BilinearInterpolation(
00417   double* position, ignition::math::Vector3d * values, double* points) const {
00418   ignition::math::Vector3d intermediate_values[2] = { LinearInterpolation(
00419                                              position[0], &(values[0]), &(points[0])),
00420                                            LinearInterpolation(
00421                                              position[0], &(values[2]), &(points[2])) };
00422   ignition::math::Vector3d value = LinearInterpolation(
00423                           position[1], intermediate_values, &(points[4]));
00424   return value;
00425 }
00426 
00427 ignition::math::Vector3d GazeboWindPlugin::TrilinearInterpolation(
00428   ignition::math::Vector3d link_position, ignition::math::Vector3d * values, double* points) const {
00429   double position[3] = {link_position.X(),link_position.Y(),link_position.Z()};
00430   ignition::math::Vector3d intermediate_values[4] = { LinearInterpolation(
00431                                              position[2], &(values[0]), &(points[0])),
00432                                            LinearInterpolation(
00433                                              position[2], &(values[2]), &(points[2])),
00434                                            LinearInterpolation(
00435                                              position[2], &(values[4]), &(points[4])),
00436                                            LinearInterpolation(
00437                                              position[2], &(values[6]), &(points[6])) };
00438   ignition::math::Vector3d value = BilinearInterpolation(
00439     &(position[0]), intermediate_values, &(points[8]));
00440   return value;
00441 }
00442 
00443 GZ_REGISTER_MODEL_PLUGIN(GazeboWindPlugin);
00444 
00445 }  // namespace gazebo


rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43