quadrotor_teleop.cpp
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 
00030 #include <ros/ros.h>
00031 #include <sensor_msgs/Joy.h>
00032 #include <geometry_msgs/Twist.h>
00033 #include <hector_uav_msgs/YawrateCommand.h>
00034 #include <hector_uav_msgs/ThrustCommand.h>
00035 #include <hector_uav_msgs/AttitudeCommand.h>
00036 
00037 namespace hector_quadrotor
00038 {
00039 
00040 class Teleop
00041 {
00042 private:
00043   ros::NodeHandle node_handle_;
00044   ros::Subscriber joy_subscriber_;
00045 
00046   ros::Publisher velocity_publisher_, attitude_publisher_, yawrate_publisher_, thrust_publisher_;
00047   geometry_msgs::Twist velocity_;
00048   hector_uav_msgs::AttitudeCommand attitude_;
00049   hector_uav_msgs::ThrustCommand thrust_;
00050   hector_uav_msgs::YawrateCommand yawrate_;
00051 
00052   struct Axis
00053   {
00054     int axis;
00055     double max;
00056   };
00057 
00058   struct Button
00059   {
00060     int button;
00061   };
00062 
00063   struct
00064   {
00065     Axis x;
00066     Axis y;
00067     Axis z;
00068     Axis yaw;
00069   } axes_;
00070 
00071   struct
00072   {
00073     Button slow;
00074   } buttons_;
00075 
00076   double slow_factor_;
00077 
00078 public:
00079   Teleop()
00080   {
00081     ros::NodeHandle params("~");
00082 
00083     params.param<int>("x_axis", axes_.x.axis, 4);
00084     params.param<int>("y_axis", axes_.y.axis, 3);
00085     params.param<int>("z_axis", axes_.z.axis, 2);
00086     params.param<int>("yaw_axis", axes_.yaw.axis, 1);
00087 
00088     params.param<double>("yaw_velocity_max", axes_.yaw.max, 90.0 * M_PI / 180.0);
00089     params.param<int>("slow_button", buttons_.slow.button, 1);
00090     params.param<double>("slow_factor", slow_factor_, 0.2);
00091 
00092     std::string control_mode_str;
00093     params.param<std::string>("control_mode", control_mode_str, "twist");
00094 
00095     if (control_mode_str == "twist")
00096     {
00097       params.param<double>("x_velocity_max", axes_.x.max, 2.0);
00098       params.param<double>("y_velocity_max", axes_.y.max, 2.0);
00099       params.param<double>("z_velocity_max", axes_.z.max, 2.0);
00100 
00101       joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyTwistCallback, this, _1));
00102       velocity_publisher_ = node_handle_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
00103     }
00104     else if (control_mode_str == "attitude")
00105     {
00106       params.param<double>("x_roll_max", axes_.x.max, 0.35);
00107       params.param<double>("y_pitch_max", axes_.y.max, 0.35);
00108       params.param<double>("z_thrust_max", axes_.z.max, 25.0);
00109       joy_subscriber_ = node_handle_.subscribe<sensor_msgs::Joy>("joy", 1, boost::bind(&Teleop::joyAttitudeCallback, this, _1));
00110       attitude_publisher_ = node_handle_.advertise<hector_uav_msgs::AttitudeCommand>("command/attitude", 10);
00111       yawrate_publisher_ = node_handle_.advertise<hector_uav_msgs::YawrateCommand>("command/yawrate", 10);
00112       thrust_publisher_ = node_handle_.advertise<hector_uav_msgs::ThrustCommand>("command/thrust", 10);
00113     }
00114 
00115   }
00116 
00117   ~Teleop()
00118   {
00119     stop();
00120   }
00121 
00122   void joyTwistCallback(const sensor_msgs::JoyConstPtr &joy)
00123   {
00124     velocity_.linear.x = getAxis(joy, axes_.x);
00125     velocity_.linear.y = getAxis(joy, axes_.y);
00126     velocity_.linear.z = getAxis(joy, axes_.z);
00127     velocity_.angular.z = getAxis(joy, axes_.yaw);
00128     if (getButton(joy, buttons_.slow.button))
00129     {
00130       velocity_.linear.x *= slow_factor_;
00131       velocity_.linear.y *= slow_factor_;
00132       velocity_.linear.z *= slow_factor_;
00133       velocity_.angular.z *= slow_factor_;
00134     }
00135     velocity_publisher_.publish(velocity_);
00136   }
00137 
00138   void joyAttitudeCallback(const sensor_msgs::JoyConstPtr &joy)
00139   {
00140     attitude_.roll = getAxis(joy, axes_.x);
00141     attitude_.pitch = getAxis(joy, axes_.y);
00142     attitude_publisher_.publish(attitude_);
00143 
00144     thrust_.thrust = getAxis(joy, axes_.z);
00145     thrust_publisher_.publish(thrust_);
00146 
00147     yawrate_.turnrate = getAxis(joy, axes_.yaw);
00148     if (getButton(joy, buttons_.slow.button))
00149     {
00150       yawrate_.turnrate *= slow_factor_;
00151     }
00152     yawrate_publisher_.publish(yawrate_);
00153   }
00154 
00155   sensor_msgs::Joy::_axes_type::value_type getAxis(const sensor_msgs::JoyConstPtr &joy, Axis axis)
00156   {
00157     if (axis.axis == 0)
00158     {return 0;}
00159     sensor_msgs::Joy::_axes_type::value_type sign = 1.0;
00160     if (axis.axis < 0)
00161     {
00162       sign = -1.0;
00163       axis.axis = -axis.axis;
00164     }
00165     if ((size_t) axis.axis > joy->axes.size())
00166     {return 0;}
00167     return sign * joy->axes[axis.axis - 1] * axis.max;
00168   }
00169 
00170   sensor_msgs::Joy::_buttons_type::value_type getButton(const sensor_msgs::JoyConstPtr &joy, int button)
00171   {
00172     if (button <= 0)
00173     {return 0;}
00174     if ((size_t) button > joy->buttons.size())
00175     {return 0;}
00176     return joy->buttons[button - 1];
00177   }
00178 
00179   void stop()
00180   {
00181     if(velocity_publisher_.getNumSubscribers() > 0)
00182     {
00183       velocity_ = geometry_msgs::Twist();
00184       velocity_publisher_.publish(velocity_);
00185     }
00186     if(attitude_publisher_.getNumSubscribers() > 0)
00187     {
00188       attitude_ = hector_uav_msgs::AttitudeCommand();
00189       attitude_publisher_.publish(attitude_);
00190     }
00191     if(thrust_publisher_.getNumSubscribers() > 0)
00192     {
00193       thrust_ = hector_uav_msgs::ThrustCommand();
00194       thrust_publisher_.publish(thrust_);
00195     }
00196     if(yawrate_publisher_.getNumSubscribers() > 0)
00197     {
00198       yawrate_ = hector_uav_msgs::YawrateCommand();
00199       yawrate_publisher_.publish(yawrate_);
00200     }
00201 
00202   }
00203 };
00204 
00205 } // namespace hector_quadrotor
00206 
00207 int main(int argc, char **argv)
00208 {
00209   ros::init(argc, argv, "quadrotor_teleop");
00210 
00211   hector_quadrotor::Teleop teleop;
00212   ros::spin();
00213 
00214   return 0;
00215 }


hector_quadrotor_teleop
Author(s): Johannes Meyer
autogenerated on Sun Jul 10 2016 04:50:11