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 #ifndef USV_GAZEBO_DYNAMICS_H
00024 #define USV_GAZEBO_DYNAMICS_H
00025
00026
00027 #include <vector>
00028 #include <iostream>
00029 #include <sstream>
00030
00031
00032 #include <gazebo/common/common.hh>
00033 #include <gazebo/physics/physics.hh>
00034
00035
00036
00037 #include <nav_msgs/Odometry.h>
00038 #include <geometry_msgs/TwistWithCovariance.h>
00039 #include <geometry_msgs/PoseWithCovariance.h>
00040
00041
00042
00043 #include <Eigen/Core>
00044
00045 #include <ros/ros.h>
00046
00047
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 ¶m_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
00088
00089 event::ConnectionPtr update_connection_;
00090
00092 physics::WorldPtr world_;
00094 physics::ModelPtr model_;
00098 physics::LinkPtr link_;
00099
00100
00101
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
00121 float buoy_frac_;
00122 float dx_;
00123 float dy_;
00124 std::vector<int> II_;
00125
00126
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
00167 double water_level_;
00168
00169 double water_density_;
00171 Eigen::MatrixXd Ma_;
00172
00173
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
00179
00180
00181
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
00195 event::ConnectionPtr updateConnection;
00196
00197 };
00198 }
00199
00200 #endif //USV_GAZEBO_DYNAMICS_H