usv_gazebo_dynamics_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 Foobar.  If not, see <http://www.gnu.org/licenses/>.
00020 
00021 */
00022 
00023 #ifndef USV_GAZEBO_DYNAMICS_H
00024 #define USV_GAZEBO_DYNAMICS_H
00025 
00026 // C++
00027 #include <vector>
00028 #include <iostream>     // std::cout, std::ios
00029 #include <sstream>      // std::ostringstream
00030 
00031 // Gazebo
00032 #include <gazebo/common/common.hh>
00033 #include <gazebo/physics/physics.hh>
00034 //#include <gazebo_plugins/gazebo_ros_utils.h>
00035 
00036 //ROS
00037 #include <nav_msgs/Odometry.h>
00038 #include <geometry_msgs/TwistWithCovariance.h>
00039 #include <geometry_msgs/PoseWithCovariance.h>
00040 //#include <usv_msgs/UsvDrive.h>
00041 //#include <usv_gazebo_plugins/UsvDrive.h>
00042 
00043 #include <Eigen/Core>
00044                                     //#include <tf/transform_broadcaster.h>
00045 #include <ros/ros.h>
00046 
00047 // Custom Callback Queue
00048 #include <ros/callback_queue.h>
00049 #include <ros/advertise_options.h>
00050 
00051 namespace gazebo
00052 {
00053   class UsvPlugin : public ModelPlugin
00054   {
00055   public:
00056     UsvPlugin();
00057     virtual ~UsvPlugin();
00059     virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00060   protected:
00062     virtual void UpdateChild();
00063     virtual void FiniChild();
00064   private:
00067     void OnContact(const std::string &name, const physics::Contact &contact);
00068 
00070     void spin();
00071 
00075     double getSdfParamDouble(sdf::ElementPtr sdfPtr,const std::string &param_name,double default_val);
00076 
00078     std::string node_namespace_;
00079     std::string link_name_;
00080     
00081     ros::NodeHandle *rosnode_;
00082     
00083     ros::Publisher sensor_state_pub_;
00084     ros::Publisher odom_pub_;
00085     ros::Publisher joint_state_pub_;
00086     
00087     //GazeboRosPtr gazebo_ros_;
00088     //physics::ModelPtr parent;
00089     event::ConnectionPtr update_connection_;
00090 
00092     physics::WorldPtr world_;
00094     physics::ModelPtr model_;  
00098     physics::LinkPtr link_;
00099     
00100     
00101     // Simulation time of the last update
00102     common::Time prev_update_time_;
00103     math::Vector3 prev_lin_vel_;
00104     math::Vector3 prev_ang_vel_;
00105     math::Pose pose_;
00106     math::Vector3 euler_;
00107     math::Vector3 vel_linear_body_;
00108     math::Vector3 vel_angular_body_;
00109     math::Vector3 acceleration;
00110     math::Vector3 angular_velocity_;
00111     math::Vector3 angular_acceleration_;
00112     Eigen::VectorXd state_dot_;
00113     Eigen::VectorXd state_;
00114     Eigen::VectorXd amassVec_;
00115     Eigen::MatrixXd Cmat_;
00116     Eigen::VectorXd Cvec_;
00117     Eigen::MatrixXd Dmat_;
00118     Eigen::VectorXd Dvec_;
00119 
00120     // For Buoyancy calculation
00121     float buoy_frac_;
00122     float dx_;
00123     float dy_;
00124     std::vector<int> II_;
00125     
00126     // Values to set via Plugin Parameters
00128     double param_X_dot_u_;
00130     double param_Y_dot_v_;
00132     double param_N_dot_r_;
00133 
00135     double param_X_u_;
00137     double param_X_uu_;
00139     double param_Y_v_;
00141     double param_Y_vv_;
00142     
00143     double param_Z_w_;
00144     double param_K_p_;
00145     double param_M_q_;
00146 
00148     double param_N_r_;
00150     double param_N_rr_;
00152     double param_boat_width_;
00154     double param_boat_length_;
00156     double param_boat_area_ ;
00158     double param_metacentric_length_;
00160     double param_metacentric_width_;
00161 
00162 
00163     double xyz_damping_;
00164     double yaw_damping_;
00165     double rp_damping_;
00166     /* Water height [m]*/
00167     double water_level_;
00168     /* Water density [kg/m^3] */
00169     double water_density_;
00171     Eigen::MatrixXd Ma_;
00172     
00173     /* Wave parameters */
00174     int param_wave_n_;
00175     std::vector<float> param_wave_amps_;
00176     std::vector<float> param_wave_periods_;
00177     std::vector< std::vector<float> > param_wave_directions_;
00178     /* Old - for single wave
00179     double param_wave_amp_;
00180     math::Vector2d param_wave_dir_;
00181     double param_wave_period_;
00182     */
00183 
00185     math::Vector3 param_wind_velocity_vector_;
00186 
00188     math::Vector3 param_wind_coeff_vector_;
00189     
00190     boost::thread *spinner_thread_;
00191     
00192     event::ConnectionPtr contact_event_;
00193     
00194     // Pointer to the update event connection
00195     event::ConnectionPtr updateConnection;
00196 
00197   };  // class UsvPlugin
00198 } // namespace gazebo
00199 
00200 #endif //USV_GAZEBO_DYNAMICS_H


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