usv_gazebo_wind_plugin.cpp
Go to the documentation of this file.
00001 /* 
00002 
00003 Copyright (c) 2018, Brian Bingham
00004 All rights reserved
00005 
00006 This file is part of the usv_gazebo_dynamics_plugin package, known as this Package.
00007 
00008 This Package free software: you can redistribute it and/or modify
00009 it under the terms of the GNU General Public License as published by
00010 the Free Software Foundation, either version 3 of the License, or
00011 (at your option) any later version.
00012 
00013 This Package s distributed in the hope that it will be useful,
00014 but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016 GNU General Public License for more details.
00017 
00018 You should have received a copy of the GNU General Public License
00019 along with this package.  If not, see <http://www.gnu.org/licenses/>.
00020 
00021 */
00022 
00023 #include <boost/thread.hpp>
00024 #include <nav_msgs/Odometry.h>
00025 #include <sensor_msgs/JointState.h>
00026 #include <ros/time.h>
00027 #include <tf2/LinearMath/Transform.h>
00028 
00029 #include <usv_gazebo_plugins/usv_gazebo_wind_plugin.h>
00030 
00031 #define GRAVITY 9.815
00032 
00033 using namespace gazebo;
00034 
00035 UsvWindPlugin::UsvWindPlugin()
00036 {
00037 }
00038 
00039 UsvWindPlugin::~UsvWindPlugin()
00040 {
00041   rosnode_->shutdown();
00042   spinner_thread_->join();
00043   delete rosnode_;
00044   delete spinner_thread_;
00045 }
00046 
00047 void UsvWindPlugin::FiniChild()
00048 {
00049 }
00050     
00051 
00052 double UsvWindPlugin::getSdfParamDouble(sdf::ElementPtr sdfPtr, const std::string &param_name, double default_val)
00053 {
00054   double val = default_val;
00055   if (sdfPtr->HasElement(param_name) && sdfPtr->GetElement(param_name)->GetValue()) 
00056   {
00057     val = sdfPtr->GetElement(param_name)->Get<double>();
00058     ROS_INFO_STREAM("Parameter found - setting <" << param_name << "> to <" << val << ">.");
00059 
00060   }
00061   else{
00062     ROS_INFO_STREAM("Parameter <" << param_name << "> not found: Using default value of <" << val << ">.");
00063   }
00064   return val;
00065 }
00066                                     
00067 void UsvWindPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00068 {
00069   ROS_INFO("Loading usv_gazebo_wind_plugin");
00070   model_ = _parent;
00071   world_ = model_->GetWorld();
00072   
00073   // Retrieve model paramters from SDF
00074   // Set default values
00075   node_namespace_ = "";
00076 
00077   //  Enumerating model
00078   ROS_INFO_STREAM("Enumerating Model...");
00079   ROS_INFO_STREAM("Model name = "<< model_->GetName());
00080   physics::Link_V links = model_->GetLinks();
00081   for (int ii=0; ii<links.size(); ii++){
00082     ROS_INFO_STREAM("Link: "<<links[ii]->GetName());
00083   }
00084 
00085   // Get parameters from SDF
00086   if (_sdf->HasElement("robotNamespace")) 
00087   {
00088     node_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00089   }
00090 
00091   if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
00092   {                       
00093     link_ = model_->GetLink();
00094     link_name_ = link_->GetName();
00095     ROS_INFO_STREAM("Did not find SDF parameter bodyName");
00096   } 
00097   else {
00098     link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00099     //link_name_ = "thrust_link";
00100     link_ = model_->GetLink(link_name_);
00101 
00102     ROS_INFO_STREAM("Found SDF parameter bodyName as <"<<link_name_<<">");
00103   }
00104   if (!link_)
00105   {
00106     ROS_FATAL("usv_gazebo_wind_plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00107     return;
00108   }
00109   else
00110   {
00111     ROS_INFO_STREAM("USV Model Link Name = " << link_name_);
00112   }
00113 
00114   if (_sdf->HasElement("wind_velocity_vector")){
00115     param_wind_velocity_vector_ = _sdf->GetElement("wind_velocity_vector")->Get<math::Vector3>();
00116   }
00117   else{
00118     param_wind_velocity_vector_ = math::Vector3(0,0,0);
00119   }
00120   ROS_INFO_STREAM("Wind velocity vector = "<<param_wind_velocity_vector_.x << " , " << param_wind_velocity_vector_.y << " , " << param_wind_velocity_vector_.z);
00121 
00122 
00123   if (_sdf->HasElement("wind_coeff_vector")){
00124     param_wind_coeff_vector_ = _sdf->GetElement("wind_coeff_vector")->Get<math::Vector3>();
00125   }
00126   else{
00127     param_wind_coeff_vector_ = math::Vector3(0,0,0);
00128   }
00129   ROS_INFO_STREAM("Wind coefficient vector = "<<param_wind_coeff_vector_.x << " , " << param_wind_coeff_vector_.y << " , " << param_wind_coeff_vector_.z);
00130 
00131 
00132   // Initialize the ROS node and subscribe to cmd_drive
00133   int argc = 0;
00134   char** argv = NULL;
00135   ros::init(argc, argv, "usv_wind_gazebo", 
00136             ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00137   rosnode_ = new ros::NodeHandle( node_namespace_ );
00138 
00139   // Listen to the update event. This event is broadcast every
00140   // simulation iteration.
00141   this->spinner_thread_ = new boost::thread( boost::bind( &UsvWindPlugin::spin, this) );
00142   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00143     boost::bind(&UsvWindPlugin::UpdateChild, this));
00144 }
00145 void UsvWindPlugin::UpdateChild()
00146 {
00147   // Wind
00148   // Transform wind from world coordinates to body coordinates
00149   math::Vector3 relative_wind = link_->GetWorldPose().rot.GetInverse().RotateVector(param_wind_velocity_vector_);
00150   // Calculate apparent wind
00151   math::Vector3 apparent_wind = relative_wind - link_->GetRelativeLinearVel();
00152   ROS_DEBUG_STREAM_THROTTLE(1.0,"Relative wind: " << relative_wind);
00153   ROS_DEBUG_STREAM_THROTTLE(1.0,"Apparent wind: " << apparent_wind);
00154   // Calculate wind force - body coordinates
00155   math::Vector3 wind_force(
00156                            param_wind_coeff_vector_.x * relative_wind.x * abs(relative_wind.x),
00157                            param_wind_coeff_vector_.y * relative_wind.y * abs(relative_wind.y),
00158                            -2.0*param_wind_coeff_vector_.z * relative_wind.x * relative_wind.y);
00159   
00160   
00161   // Add forces/torques to link at CG
00162   link_->AddRelativeForce(math::Vector3(wind_force.x, wind_force.y, 0.0));
00163   link_->AddRelativeTorque(math::Vector3(0.0,0.0,wind_force.z));  
00164 }
00165 
00166 void UsvWindPlugin::spin()
00167 {
00168     while(ros::ok()) ros::spinOnce();
00169 }
00170 
00171 GZ_REGISTER_MODEL_PLUGIN(UsvWindPlugin);
00172 


usv_gazebo_plugins
Author(s): Brian Bingham
autogenerated on Thu Mar 7 2019 03:37:04