controller.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf/transform_listener.h>
00003 #include <std_srvs/Empty.h>
00004 #include <geometry_msgs/Twist.h>
00005 
00006 
00007 #include "pid.hpp"
00008 
00009 double get(
00010     const ros::NodeHandle& n,
00011     const std::string& name) {
00012     double value;
00013     n.getParam(name, value);
00014     return value;
00015 }
00016 
00017 class Controller
00018 {
00019 public:
00020 
00021     Controller(
00022         const std::string& worldFrame,
00023         const std::string& frame,
00024         const ros::NodeHandle& n)
00025         : m_worldFrame(worldFrame)
00026         , m_frame(frame)
00027         , m_pubNav()
00028         , m_listener()
00029         , m_pidX(
00030             get(n, "PIDs/X/kp"),
00031             get(n, "PIDs/X/kd"),
00032             get(n, "PIDs/X/ki"),
00033             get(n, "PIDs/X/minOutput"),
00034             get(n, "PIDs/X/maxOutput"),
00035             get(n, "PIDs/X/integratorMin"),
00036             get(n, "PIDs/X/integratorMax"),
00037             "x")
00038         , m_pidY(
00039             get(n, "PIDs/Y/kp"),
00040             get(n, "PIDs/Y/kd"),
00041             get(n, "PIDs/Y/ki"),
00042             get(n, "PIDs/Y/minOutput"),
00043             get(n, "PIDs/Y/maxOutput"),
00044             get(n, "PIDs/Y/integratorMin"),
00045             get(n, "PIDs/Y/integratorMax"),
00046             "y")
00047         , m_pidZ(
00048             get(n, "PIDs/Z/kp"),
00049             get(n, "PIDs/Z/kd"),
00050             get(n, "PIDs/Z/ki"),
00051             get(n, "PIDs/Z/minOutput"),
00052             get(n, "PIDs/Z/maxOutput"),
00053             get(n, "PIDs/Z/integratorMin"),
00054             get(n, "PIDs/Z/integratorMax"),
00055             "z")
00056         , m_pidYaw(
00057             get(n, "PIDs/Yaw/kp"),
00058             get(n, "PIDs/Yaw/kd"),
00059             get(n, "PIDs/Yaw/ki"),
00060             get(n, "PIDs/Yaw/minOutput"),
00061             get(n, "PIDs/Yaw/maxOutput"),
00062             get(n, "PIDs/Yaw/integratorMin"),
00063             get(n, "PIDs/Yaw/integratorMax"),
00064             "yaw")
00065         , m_state(Idle)
00066         , m_goal()
00067         , m_subscribeGoal()
00068         , m_serviceTakeoff()
00069         , m_serviceLand()
00070         , m_thrust(0)
00071         , m_startZ(0)
00072     {
00073         ros::NodeHandle nh;
00074         m_listener.waitForTransform(m_worldFrame, m_frame, ros::Time(0), ros::Duration(10.0)); 
00075         m_pubNav = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00076         m_subscribeGoal = nh.subscribe("goal", 1, &Controller::goalChanged, this);
00077         m_serviceTakeoff = nh.advertiseService("takeoff", &Controller::takeoff, this);
00078         m_serviceLand = nh.advertiseService("land", &Controller::land, this);
00079     }
00080 
00081     void run(double frequency)
00082     {
00083         ros::NodeHandle node;
00084         ros::Timer timer = node.createTimer(ros::Duration(1.0/frequency), &Controller::iteration, this);
00085         ros::spin();
00086     }
00087 
00088 private:
00089     void goalChanged(
00090         const geometry_msgs::PoseStamped::ConstPtr& msg)
00091     {
00092         m_goal = *msg;
00093     }
00094 
00095     bool takeoff(
00096         std_srvs::Empty::Request& req,
00097         std_srvs::Empty::Response& res)
00098     {
00099         ROS_INFO("Takeoff requested!");
00100         m_state = TakingOff;
00101 
00102         tf::StampedTransform transform;
00103         m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);
00104         m_startZ = transform.getOrigin().z();
00105 
00106         return true;
00107     }
00108 
00109     bool land(
00110         std_srvs::Empty::Request& req,
00111         std_srvs::Empty::Response& res)
00112     {
00113         ROS_INFO("Landing requested!");
00114         m_state = Landing;
00115 
00116         return true;
00117     }
00118 
00119     void getTransform(
00120         const std::string& sourceFrame,
00121         const std::string& targetFrame,
00122         tf::StampedTransform& result)
00123     {
00124         m_listener.lookupTransform(sourceFrame, targetFrame, ros::Time(0), result);
00125     }
00126 
00127     void pidReset()
00128     {
00129         m_pidX.reset();
00130         m_pidZ.reset();
00131         m_pidZ.reset();
00132         m_pidYaw.reset();
00133     }
00134 
00135     void iteration(const ros::TimerEvent& e)
00136     {
00137         float dt = e.current_real.toSec() - e.last_real.toSec();
00138 
00139         switch(m_state)
00140         {
00141         case TakingOff:
00142             {
00143                 tf::StampedTransform transform;
00144                 m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);
00145                 if (transform.getOrigin().z() > m_startZ + 0.05 || m_thrust > 50000)
00146                 {
00147                     pidReset();
00148                     m_pidZ.setIntegral(m_thrust / m_pidZ.ki());
00149                     m_state = Automatic;
00150                     m_thrust = 0;
00151                 }
00152                 else
00153                 {
00154                     m_thrust += 10000 * dt;
00155                     geometry_msgs::Twist msg;
00156                     msg.linear.z = m_thrust;
00157                     m_pubNav.publish(msg);
00158                 }
00159 
00160             }
00161             break;
00162         case Landing:
00163             {
00164                 m_goal.pose.position.z = m_startZ + 0.05;
00165                 tf::StampedTransform transform;
00166                 m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);
00167                 if (transform.getOrigin().z() <= m_startZ + 0.05) {
00168                     m_state = Idle;
00169                     geometry_msgs::Twist msg;
00170                     m_pubNav.publish(msg);
00171                 }
00172             }
00173             // intentional fall-thru
00174         case Automatic:
00175             {
00176                 tf::StampedTransform transform;
00177                 m_listener.lookupTransform(m_worldFrame, m_frame, ros::Time(0), transform);
00178 
00179                 geometry_msgs::PoseStamped targetWorld;
00180                 targetWorld.header.stamp = transform.stamp_;
00181                 targetWorld.header.frame_id = m_worldFrame;
00182                 targetWorld.pose = m_goal.pose;
00183 
00184                 geometry_msgs::PoseStamped targetDrone;
00185                 m_listener.transformPose(m_frame, targetWorld, targetDrone);
00186 
00187                 tfScalar roll, pitch, yaw;
00188                 tf::Matrix3x3(
00189                     tf::Quaternion(
00190                         targetDrone.pose.orientation.x,
00191                         targetDrone.pose.orientation.y,
00192                         targetDrone.pose.orientation.z,
00193                         targetDrone.pose.orientation.w
00194                     )).getRPY(roll, pitch, yaw);
00195 
00196                 geometry_msgs::Twist msg;
00197                 msg.linear.x = m_pidX.update(0, targetDrone.pose.position.x);
00198                 msg.linear.y = m_pidY.update(0.0, targetDrone.pose.position.y);
00199                 msg.linear.z = m_pidZ.update(0.0, targetDrone.pose.position.z);
00200                 msg.angular.z = m_pidYaw.update(0.0, yaw);
00201                 m_pubNav.publish(msg);
00202 
00203 
00204             }
00205             break;
00206         case Idle:
00207             {
00208                 geometry_msgs::Twist msg;
00209                 m_pubNav.publish(msg);
00210             }
00211             break;
00212         }
00213     }
00214 
00215 private:
00216 
00217     enum State
00218     {
00219         Idle = 0,
00220         Automatic = 1,
00221         TakingOff = 2,
00222         Landing = 3,
00223     };
00224 
00225 private:
00226     std::string m_worldFrame;
00227     std::string m_frame;
00228     ros::Publisher m_pubNav;
00229     tf::TransformListener m_listener;
00230     PID m_pidX;
00231     PID m_pidY;
00232     PID m_pidZ;
00233     PID m_pidYaw;
00234     State m_state;
00235     geometry_msgs::PoseStamped m_goal;
00236     ros::Subscriber m_subscribeGoal;
00237     ros::ServiceServer m_serviceTakeoff;
00238     ros::ServiceServer m_serviceLand;
00239     float m_thrust;
00240     float m_startZ;
00241 };
00242 
00243 int main(int argc, char **argv)
00244 {
00245   ros::init(argc, argv, "controller");
00246 
00247   // Read parameters
00248   ros::NodeHandle n("~");
00249   std::string worldFrame;
00250   n.param<std::string>("worldFrame", worldFrame, "/world");
00251   std::string frame;
00252   n.getParam("frame", frame);
00253   double frequency;
00254   n.param("frequency", frequency, 50.0);
00255 
00256   Controller controller(worldFrame, frame, n);
00257   controller.run(frequency);
00258 
00259   return 0;
00260 }


crazyflie_controller
Author(s): Wolfgang Hoenig
autogenerated on Sun Oct 8 2017 02:47:57