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 #include <std_srvs/Empty.h>
00041
00042 #include <hector_gazebo_plugins/update_timer.h>
00043
00044 namespace gazebo
00045 {
00046
00047 class GazeboQuadrotorSimpleController : public ModelPlugin
00048 {
00049 public:
00050 GazeboQuadrotorSimpleController();
00051 virtual ~GazeboQuadrotorSimpleController();
00052
00053 protected:
00054 virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00055 virtual void Update();
00056 virtual void Reset();
00057
00058 private:
00060 physics::WorldPtr world;
00061
00063 physics::LinkPtr link;
00064
00065 ros::NodeHandle* node_handle_;
00066 ros::CallbackQueue callback_queue_;
00067 ros::Subscriber velocity_subscriber_;
00068 ros::Subscriber imu_subscriber_;
00069 ros::Subscriber state_subscriber_;
00070 ros::Publisher wrench_publisher_;
00071
00072 ros::ServiceServer engage_service_server_;
00073 ros::ServiceServer shutdown_service_server_;
00074
00075
00076
00077
00078
00079 geometry_msgs::Twist velocity_command_;
00080 void VelocityCallback(const geometry_msgs::TwistConstPtr&);
00081 void ImuCallback(const sensor_msgs::ImuConstPtr&);
00082 void StateCallback(const nav_msgs::OdometryConstPtr&);
00083
00084 bool EngageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00085 bool ShutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
00086
00087 ros::Time state_stamp;
00088 math::Pose pose;
00089 math::Vector3 euler, velocity, acceleration, angular_velocity;
00090
00091 std::string link_name_;
00092 std::string namespace_;
00093 std::string velocity_topic_;
00094 std::string imu_topic_;
00095 std::string state_topic_;
00096 std::string wrench_topic_;
00097 double max_force_;
00098
00099 bool running_;
00100 bool auto_engage_;
00101
00102 class PIDController {
00103 public:
00104 PIDController();
00105 virtual ~PIDController();
00106 virtual void Load(sdf::ElementPtr _sdf, const std::string& prefix = "");
00107
00108 double gain_p;
00109 double gain_i;
00110 double gain_d;
00111 double time_constant;
00112 double limit;
00113
00114 double input;
00115 double dinput;
00116 double output;
00117 double p, i, d;
00118
00119 double update(double input, double x, double dx, double dt);
00120 void reset();
00121 };
00122
00123 struct Controllers {
00124 PIDController roll;
00125 PIDController pitch;
00126 PIDController yaw;
00127 PIDController velocity_x;
00128 PIDController velocity_y;
00129 PIDController velocity_z;
00130 } controllers_;
00131
00132 math::Vector3 inertia;
00133 double mass;
00134
00135 math::Vector3 force, torque;
00136
00137 UpdateTimer controlTimer;
00138 event::ConnectionPtr updateConnection;
00139 };
00140
00141 }
00142
00143 #endif // HECTOR_QUADROTOR_GAZEBO_PLUGINS_QUADROTOR_SIMPLE_CONTROLLER_H