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 <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 ¶m_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
00069
00070 node_namespace_ = "";
00071 cmd_timeout_ = 1.0;
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
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
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
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
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_);
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
00142 prev_update_time_ = last_cmd_drive_time_ = this->world_->GetSimTime();
00143
00144
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
00154
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
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
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
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:
00222 thrust_left = scaleThrustCmd(last_cmd_drive_left_);
00223 thrust_right = scaleThrustCmd(last_cmd_drive_right_);
00224 break;
00225 case 1:
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 }
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
00242 link_->AddRelativeTorque(math::Vector3(0,0,torque));
00243
00244
00245 math::Vector3 relpos(-1.0*param_boat_length_/2.0, 0.0 ,
00246 param_thrust_z_offset_);
00247 math::Vector3 inputforce3(thrust, 0,0);
00248
00249
00250 pose_ = link_->GetWorldPose();
00251
00252
00253 inputforce3 = pose_.rot.RotateVector(inputforce3);
00254
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