usv_gazebo_thrust_plugin.h
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 #ifndef USV_GAZEBO_THRUST_H
00024 #define USV_GAZEBO_THRUST_H
00025 
00026 // C++
00027 #include <algorithm>  // min/mzx
00028 #include <math.h>
00029 
00030 // Gazebo
00031 #include <gazebo/common/common.hh>
00032 #include <gazebo/physics/physics.hh>
00033 //#include <gazebo_plugins/gazebo_ros_utils.h>
00034 
00035 //ROS
00036 #include <usv_gazebo_plugins/UsvDrive.h>
00037 #include <ros/ros.h>
00038 
00039 // Custom Callback Queue
00040 #include <ros/callback_queue.h>
00041 #include <ros/advertise_options.h>
00042 
00043 namespace gazebo
00044 {
00045   class UsvThrust : public ModelPlugin
00046   {
00047   public:
00048     UsvThrust();
00049     virtual ~UsvThrust();
00051     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00052   protected:
00054     virtual void UpdateChild();
00055     virtual void FiniChild();
00056   private:
00061     void OnCmdDrive( const usv_gazebo_plugins::UsvDriveConstPtr &msg);
00062 
00064     void spin();
00065 
00069     double getSdfParamDouble(sdf::ElementPtr sdfPtr,const std::string &param_name,double default_val);
00070 
00079     double scaleThrustCmd(double cmd);
00080 
00081     double glf(double x, float A, float K, float B,
00082                float v, float C, float M);
00083     
00084     double glfThrustCmd(double cmd);
00085 
00087     std::string node_namespace_;
00088     std::string link_name_;
00089     
00090     ros::NodeHandle *rosnode_;
00091     
00092     ros::Subscriber cmd_drive_sub_;
00093     
00094     //GazeboRosPtr gazebo_ros_;
00095     //physics::ModelPtr parent;
00096     event::ConnectionPtr update_connection_;
00097     boost::thread *spinner_thread_;
00098 
00100     physics::WorldPtr world_;
00102     physics::ModelPtr model_;  
00106     physics::LinkPtr link_;
00107     math::Pose pose_;
00109     double cmd_timeout_;
00110     common::Time prev_update_time_;
00111     common::Time last_cmd_drive_time_;  
00112     double last_cmd_drive_left_;
00113     double last_cmd_drive_right_;
00114 
00115 
00116     int param_mapping_type_;
00118     double param_max_cmd_;
00120     double param_max_force_fwd_;
00122     double param_max_force_rev_;
00123 
00125     double param_boat_width_;
00127     double param_boat_length_;
00129     double param_thrust_z_offset_;
00130     // Pointer to the update event connection
00131     event::ConnectionPtr updateConnection;
00132   };  // class UsvThrust
00133 } // namespace gazebo
00134 
00135 #endif //USV_GAZEBO_THRUST_H


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