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
00024
00025
00026
00027
00028
00029 #ifndef HECTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H
00030 #define HECTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H
00031
00032 #include <gazebo/common/Plugin.hh>
00033
00034 #include <ros/callback_queue.h>
00035 #include <ros/ros.h>
00036
00037 #include <geometry_msgs/Twist.h>
00038 #include <sensor_msgs/Imu.h>
00039 #include <nav_msgs/Odometry.h>
00040
00041 #include <hector_gazebo_plugins/update_timer.h>
00042
00043 namespace gazebo
00044 {
00045
00046 class GazeboQuadrotorSimpleController : public ModelPlugin
00047 {
00048 public:
00049 GazeboQuadrotorSimpleController();
00050 virtual ~GazeboQuadrotorSimpleController();
00051
00052 protected:
00053 virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00054 virtual void Update();
00055 virtual void Reset();
00056
00057 private:
00059 physics::WorldPtr world;
00060
00062 physics::LinkPtr link;
00063
00064 ros::NodeHandle* node_handle_;
00065 ros::CallbackQueue callback_queue_;
00066 ros::Subscriber velocity_subscriber_;
00067 ros::Subscriber imu_subscriber_;
00068 ros::Subscriber state_subscriber_;
00069
00070
00071
00072
00073
00074 geometry_msgs::Twist velocity_command_;
00075 void VelocityCallback(const geometry_msgs::TwistConstPtr&);
00076 void ImuCallback(const sensor_msgs::ImuConstPtr&);
00077 void StateCallback(const nav_msgs::OdometryConstPtr&);
00078
00079 ros::Time state_stamp;
00080 math::Pose pose;
00081 math::Vector3 euler, velocity, acceleration, angular_velocity;
00082
00083 std::string link_name_;
00084 std::string namespace_;
00085 std::string velocity_topic_;
00086 std::string imu_topic_;
00087 std::string state_topic_;
00088 double max_force_;
00089
00090 class PIDController {
00091 public:
00092 PIDController();
00093 virtual ~PIDController();
00094 virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = "");
00095
00096 double gain_p;
00097 double gain_i;
00098 double gain_d;
00099 double time_constant;
00100 double limit;
00101
00102 double input;
00103 double dinput;
00104 double output;
00105 double p, i, d;
00106
00107 double update(double input, double x, double dx, double dt);
00108 void reset();
00109 };
00110
00111 struct Controllers {
00112 PIDController roll;
00113 PIDController pitch;
00114 PIDController yaw;
00115 PIDController velocity_x;
00116 PIDController velocity_y;
00117 PIDController velocity_z;
00118 } controllers_;
00119
00120 math::Vector3 inertia;
00121 double mass;
00122
00123 math::Vector3 force, torque;
00124
00125 UpdateTimer controlTimer;
00126 event::ConnectionPtr updateConnection;
00127 };
00128
00129 }
00130
00131 #endif // HECTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H