Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
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 ¶m_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
00074
00075 node_namespace_ = "";
00076
00077
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
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
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
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
00140
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
00148
00149 math::Vector3 relative_wind = link_->GetWorldPose().rot.GetInverse().RotateVector(param_wind_velocity_vector_);
00150
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
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
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