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_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H
00030 #define HECTOR_QUADROTOR_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 ros::Publisher wrench_publisher_;
00070
00071
00072
00073
00074
00075 geometry_msgs::Twist velocity_command_;
00076 void VelocityCallback(const geometry_msgs::TwistConstPtr&);
00077 void ImuCallback(const sensor_msgs::ImuConstPtr&);
00078 void StateCallback(const nav_msgs::OdometryConstPtr&);
00079
00080 ros::Time state_stamp;
00081 math::Pose pose;
00082 math::Vector3 euler, velocity, acceleration, angular_velocity;
00083
00084 std::string link_name_;
00085 std::string namespace_;
00086 std::string velocity_topic_;
00087 std::string imu_topic_;
00088 std::string state_topic_;
00089 std::string wrench_topic_;
00090 double max_force_;
00091
00092 class PIDController {
00093 public:
00094 PIDController();
00095 virtual ~PIDController();
00096 virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = "");
00097
00098 double gain_p;
00099 double gain_i;
00100 double gain_d;
00101 double time_constant;
00102 double limit;
00103
00104 double input;
00105 double dinput;
00106 double output;
00107 double p, i, d;
00108
00109 double update(double input, double x, double dx, double dt);
00110 void reset();
00111 };
00112
00113 struct Controllers {
00114 PIDController roll;
00115 PIDController pitch;
00116 PIDController yaw;
00117 PIDController velocity_x;
00118 PIDController velocity_y;
00119 PIDController velocity_z;
00120 } controllers_;
00121
00122 math::Vector3 inertia;
00123 double mass;
00124
00125 math::Vector3 force, torque;
00126
00127 UpdateTimer controlTimer;
00128 event::ConnectionPtr updateConnection;
00129 };
00130
00131 }
00132
00133 #endif // HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H