quadrotor_simple_controller.h
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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   // void CallbackQueueThread();
00071   // boost::mutex lock_;
00072   // boost::thread callback_queue_thread_;
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
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_quadrotor_gazebo_plugins
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:48:25