usv_gazebo_thrust_plugin.cpp
Go to the documentation of this file.
00001 /* 
00002 
00003 Copyright (c) 2017, 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 <ros/time.h>
00025 
00026 #include <usv_gazebo_plugins/usv_gazebo_thrust_plugin.h>
00027 
00028 using namespace gazebo;
00029 
00030 UsvThrust::UsvThrust()
00031 {
00032 }
00033 
00034 UsvThrust::~UsvThrust()
00035 {
00036   rosnode_->shutdown();
00037   spinner_thread_->join();
00038   delete rosnode_;
00039   delete spinner_thread_;
00040 }
00041 
00042 void UsvThrust::FiniChild()
00043 {
00044 }
00045     
00046 
00047 double UsvThrust::getSdfParamDouble(sdf::ElementPtr sdfPtr, const std::string &param_name, double default_val)
00048 {
00049   double val = default_val;
00050   if (sdfPtr->HasElement(param_name) && sdfPtr->GetElement(param_name)->GetValue()) 
00051   {
00052     val = sdfPtr->GetElement(param_name)->Get<double>();
00053     ROS_INFO_STREAM("Parameter found - setting <" << param_name << "> to <" << val << ">.");
00054 
00055   }
00056   else{
00057     ROS_INFO_STREAM("Parameter <" << param_name << "> not found: Using default value of <" << val << ">.");
00058   }
00059   return val;
00060 }
00061                                     
00062 void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
00063 {
00064   ROS_INFO("Loading usv_gazebo_thrust_plugin");
00065   model_ = _parent;
00066   world_ = model_->GetWorld();
00067   
00068   // Retrieve model paramters from SDF
00069   // Set default values
00070   node_namespace_ = "";
00071   cmd_timeout_ = 1.0; // how long to allow no input on cmd_drive
00072   param_mapping_type_ = 0;
00073   param_max_cmd_ = 1.0;
00074   param_max_force_fwd_ = 100.0;
00075   param_max_force_rev_ = -100.0;
00076   param_boat_width_ = 1.0;
00077   param_boat_length_ = 1.35;
00078   param_thrust_z_offset_ = -0.01;
00079 
00080   //  Enumerating model
00081   ROS_INFO_STREAM("Enumerating Model...");
00082   ROS_INFO_STREAM("Model name = "<< model_->GetName());
00083   physics::Link_V links = model_->GetLinks();
00084   for (unsigned int ii=0; ii<links.size(); ii++){
00085     ROS_INFO_STREAM("Link: "<<links[ii]->GetName());
00086   }
00087 
00088   // Get parameters from SDF
00089   if (_sdf->HasElement("robotNamespace")) 
00090   {
00091     node_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
00092   }
00093 
00094   if (!_sdf->HasElement("bodyName") || !_sdf->GetElement("bodyName")->GetValue())
00095   {                       
00096     link_ = model_->GetLink();
00097     link_name_ = link_->GetName();
00098     ROS_INFO_STREAM("Did not find SDF parameter bodyName");
00099   } 
00100   else {
00101     link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00102     //link_name_ = "thrust_link";
00103     link_ = model_->GetLink(link_name_);
00104 
00105     ROS_INFO_STREAM("Found SDF parameter bodyName as <"<<link_name_<<">");
00106   }
00107   if (!link_)
00108   {
00109     ROS_FATAL("usv_gazebo_thrust_plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00110     return;
00111   }
00112   else
00113   {
00114     ROS_INFO_STREAM("USV Model Link Name = " << link_name_);
00115   }
00116 
00117   cmd_timeout_ = getSdfParamDouble(_sdf,"cmdTimeout",cmd_timeout_);
00118   if (_sdf->HasElement("mappingType") && _sdf->GetElement("mappingType")->GetValue()) {
00119     param_mapping_type_ = _sdf->GetElement("mappingType")->Get<int>();
00120     ROS_INFO_STREAM("Parameter found - setting <mappingType> to <" << param_mapping_type_ <<">.");
00121   }
00122   else{
00123     ROS_INFO_STREAM("Parameter <mappingType> not found: Using default value of <" << param_mapping_type_ << ">.");
00124   }
00125   // verify
00126   if ( (param_mapping_type_ > 1) || (param_mapping_type_ < 0) ){
00127     ROS_FATAL_STREAM("Cannot use a mappingType of " << param_mapping_type_);
00128     param_mapping_type_ = 0;
00129   }
00130   param_max_cmd_ = getSdfParamDouble(_sdf,"maxCmd",param_max_cmd_);
00131   param_max_force_fwd_ = getSdfParamDouble(_sdf,"maxForceFwd",
00132                                            param_max_force_fwd_);
00133   param_max_force_rev_ = getSdfParamDouble(_sdf,"maxForceRev",
00134                                            param_max_force_rev_);
00135   param_max_force_rev_ = -1.0*std::abs(param_max_force_rev_);  // make negative
00136   param_boat_width_ = getSdfParamDouble(_sdf,"boatWidth",param_boat_width_);
00137   param_boat_length_ = getSdfParamDouble(_sdf,"boatLength",param_boat_length_);
00138   param_thrust_z_offset_ = getSdfParamDouble(_sdf,"thrustOffsetZ",
00139                                          param_thrust_z_offset_);
00140 
00141   //initialize time and odometry position
00142   prev_update_time_ = last_cmd_drive_time_ = this->world_->GetSimTime();
00143 
00144   // Initialize the ROS node and subscribe to cmd_drive
00145   int argc = 0;
00146   char** argv = NULL;
00147   ros::init(argc, argv, "usv_thrust_gazebo", 
00148             ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00149   rosnode_ = new ros::NodeHandle( node_namespace_ );
00150 
00151   cmd_drive_sub_ = rosnode_->subscribe("cmd_drive", 1, &UsvThrust::OnCmdDrive, this );
00152 
00153   // Listen to the update event. This event is broadcast every
00154   // simulation iteration.
00155   this->spinner_thread_ = new boost::thread( boost::bind( &UsvThrust::spin, this) );
00156   this->updateConnection = event::Events::ConnectWorldUpdateBegin(
00157     boost::bind(&UsvThrust::UpdateChild, this));
00158   
00159 }
00160 
00161 double UsvThrust::scaleThrustCmd(double cmd)
00162 {
00163   double val = 0.0;
00164   if (cmd >= 0.0){
00165     val = cmd/param_max_cmd_*param_max_force_fwd_;
00166     val = std::min(val,param_max_force_fwd_);
00167   }
00168   else  // cmd is less than zero
00169   {
00170     val = -1.0*std::abs(cmd)/param_max_cmd_*std::abs(param_max_force_rev_);
00171     val = std::max(val,param_max_force_rev_);
00172   }
00173   return val;  
00174 }
00175 
00176 
00177 double UsvThrust::glf(double x, float A, float K, float B,
00178                       float v, float C, float M){
00179   return A+ (K-A) / (pow(C+exp(-B*(x-M)),1.0/v));
00180 }
00181 
00182 double UsvThrust::glfThrustCmd(double cmd)
00183 {
00184   double val = 0.0;
00185   if (cmd > 0.01){
00186     val = glf(cmd,0.01,59.82,5.0,0.38,0.56,0.28);
00187     val = std::min(val,param_max_force_fwd_);
00188   }
00189   else if (cmd < 0.01){ 
00190     val = glf(cmd,-199.13,-0.09,8.84,5.34,0.99,-0.57);
00191     val = std::max(val,param_max_force_rev_);
00192   }
00193   else{
00194     val = 0.0;
00195   }
00196   ROS_INFO_STREAM_THROTTLE(0.5,cmd << ": " << val << " / " << val/param_max_force_fwd_);
00197   return val;
00198   
00199 }
00200 
00201 
00202 void UsvThrust::UpdateChild()
00203 {
00204   common::Time time_now = this->world_->GetSimTime();
00205   prev_update_time_ = time_now;
00206   
00207   // Enforce command timeout
00208   double dcmd = (time_now - last_cmd_drive_time_).Double();
00209   if ( (dcmd > cmd_timeout_) && (cmd_timeout_ > 0.0) )
00210   {
00211     ROS_INFO_STREAM_THROTTLE(1.0,"Command timeout!");
00212     last_cmd_drive_left_ = 0.0;
00213     last_cmd_drive_right_ = 0.0;
00214   }
00215   // Scale commands to thrust and torque forces
00216   ROS_DEBUG_STREAM_THROTTLE(1.0,"Last cmd: left:" << last_cmd_drive_left_
00217                    << " right: " << last_cmd_drive_right_);
00218   double thrust_left = 0.0;
00219   double thrust_right = 0.0;
00220   switch(param_mapping_type_){
00221   case 0: // Simplest, linear
00222     thrust_left = scaleThrustCmd(last_cmd_drive_left_);
00223     thrust_right = scaleThrustCmd(last_cmd_drive_right_);
00224     break;
00225   case 1: // GLF
00226     thrust_left = glfThrustCmd(last_cmd_drive_left_);
00227     thrust_right = glfThrustCmd(last_cmd_drive_right_);
00228     break;
00229   default:
00230     ROS_FATAL_STREAM("Cannot use mappingType="<<param_mapping_type_);
00231     break;
00232   
00233     
00234   } // eo switch
00235 
00236   double thrust = thrust_right + thrust_left;
00237   double torque = (thrust_right - thrust_left)*param_boat_width_;
00238   ROS_DEBUG_STREAM_THROTTLE(1.0,"Thrust: left:" << thrust_left
00239                             << " right: " << thrust_right);
00240 
00241   // Add torque
00242   link_->AddRelativeTorque(math::Vector3(0,0,torque));
00243                                          
00244   // Add input force with offset below vessel
00245   math::Vector3 relpos(-1.0*param_boat_length_/2.0, 0.0 , 
00246                        param_thrust_z_offset_);  // relative pos of thrusters
00247   math::Vector3 inputforce3(thrust, 0,0);
00248 
00249   // Get Pose
00250   pose_ = link_->GetWorldPose();
00251 
00252   //link_->AddLinkForce(inputforce3,relpos);
00253   inputforce3 = pose_.rot.RotateVector(inputforce3);
00254   //link_->AddRelativeForce(inputforce3);
00255   link_->AddForceAtRelativePosition(inputforce3,relpos);
00256 }
00257 
00258 void UsvThrust::OnCmdDrive( const usv_gazebo_plugins::UsvDriveConstPtr &msg)
00259 {
00260     last_cmd_drive_time_ = this->world_->GetSimTime();
00261     last_cmd_drive_left_ = msg->left;
00262     last_cmd_drive_right_ = msg->right;
00263 }
00264 
00265 
00266 void UsvThrust::spin()
00267 {
00268     while(ros::ok()) ros::spinOnce();
00269 }
00270 
00271 GZ_REGISTER_MODEL_PLUGIN(UsvThrust);
00272 


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