00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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
00041 model_ = _model;
00042 world_ = model_->GetWorld();
00043
00044 double wind_gust_start = kDefaultWindGustStart;
00045 double wind_gust_duration = kDefaultWindGustDuration;
00046
00047
00048
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
00057 node_handle_ = gazebo::transport::NodePtr(new transport::Node());
00058
00059
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
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
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
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
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
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
00120
00121 update_connection_ = event::Events::ConnectWorldUpdateBegin(
00122 boost::bind(&GazeboWindPlugin::OnUpdate, this, _1));
00123 }
00124
00125
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
00137 common::Time now = world_->SimTime();
00138
00139 ignition::math::Vector3d wind_velocity(0.0, 0.0, 0.0);
00140
00141
00142 if (!use_custom_static_wind_field_) {
00143
00144 double wind_strength = wind_force_mean_;
00145 ignition::math::Vector3d wind = wind_strength * wind_direction_;
00146
00147 link_->AddForceAtRelativePosition(wind, xyz_offset_);
00148
00149 ignition::math::Vector3d wind_gust(0.0, 0.0, 0.0);
00150
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
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
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
00177 wind_velocity = wind_speed_mean_ * wind_direction_;
00178 } else {
00179
00180 ignition::math::Vector3d link_position = link_->WorldPose().Pos();
00181
00182
00183
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
00188
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
00197
00198 std::size_t x_sup = x_inf + 1u;
00199 std::size_t y_sup = y_inf + 1u;
00200
00201
00202
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
00208
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
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
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
00229
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
00237 idx_z[2u * i + 1u] = 1u;
00238 } else if (vertical_factors_columns[i] >= 1u) {
00239
00240 idx_z[2u * i] = vertical_spacing_factors_.size() - 2u;
00241 } else {
00242
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
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
00263
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
00281 wind_velocity = TrilinearInterpolation(
00282 link_position, wind_at_vertices, interpolation_points);
00283 } else {
00284
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
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
00310
00311 wind_force_pub_ = node_handle_->Advertise<gz_geometry_msgs::WrenchStamped>(
00312 "~/" + namespace_ + "/" + wind_force_pub_topic_, 1);
00313
00314
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
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
00347 while (fin >> data_name) {
00348
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
00393
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 }