00001 #ifndef QUAD_JOY_TELEOP_QUAD_JOY_TELEOP_H 00002 #define QUAD_JOY_TELEOP_QUAD_JOY_TELEOP_H 00003 00004 #include <ros/ros.h> 00005 #include <sensor_msgs/Joy.h> 00006 #include <std_msgs/Bool.h> 00007 00008 #include <geometry_msgs/Twist.h> 00009 #include <geometry_msgs/TwistStamped.h> 00010 00011 #include <mav_msgs/Roll.h> 00012 #include <mav_msgs/Pitch.h> 00013 #include <mav_msgs/YawRate.h> 00014 #include <mav_msgs/Thrust.h> 00015 00016 #include <mav_srvs/ChangeDesPose.h> 00017 #include <mav_srvs/ToggleEngage.h> 00018 #include <mav_srvs/Takeoff.h> 00019 #include <mav_srvs/Land.h> 00020 #include <mav_srvs/ESTOP.h> 00021 #include <mav_srvs/PositionHold.h> 00022 #include <mav_srvs/VelocityHold.h> 00023 00024 class QuadJoyTeleop 00025 { 00026 private: 00027 00028 // **** ros-related variables 00029 00030 ros::NodeHandle nh_; 00031 ros::NodeHandle nh_private_; 00032 00033 //ros::Timer estop_timer_; 00034 00035 ros::Subscriber joy_subscriber_; 00036 ros::Publisher estop_publisher_; 00037 00038 //ros::Publisher cmd_vel_publisher_; 00039 ros::Publisher cmd_roll_publisher_; 00040 ros::Publisher cmd_pitch_publisher_; 00041 ros::Publisher cmd_yaw_rate_publisher_; 00042 ros::Publisher cmd_thrust_publisher_; 00043 ros::Publisher cmd_vel_publisher_; 00044 00045 ros::ServiceClient estop_client_; 00046 ros::ServiceClient takeoff_client_; 00047 ros::ServiceClient land_client_; 00048 ros::ServiceClient change_des_pose_client_; 00049 ros::ServiceClient toggle_engage_client_; 00050 ros::ServiceClient pos_hold_client_; 00051 ros::ServiceClient vel_hold_client_; 00052 00053 // **** parameters 00054 00055 std::string controller_; 00056 00057 int estop_button_; 00058 int takeoff_button_; 00059 int land_button_; 00060 int engage_button_; 00061 int pos_hold_button_; 00062 int vel_hold_button_; 00063 00064 int roll_axis_; 00065 int pitch_axis_; 00066 int yaw_axis_; 00067 int thrust_axis_; 00068 00069 int z_axis_; 00070 00071 int vx_axis_; 00072 int vy_axis_; 00073 int vz_axis_; 00074 int vyaw_axis_; 00075 00076 double x_step_size_; 00077 double y_step_size_; 00078 double z_step_size_; 00079 double yaw_step_size_; 00080 00081 double cmd_vel_linear_scale_; // scales stick units to m/s 00082 double cmd_vel_angular_scale_; // scales stick units to rad/s 00083 00084 double linear_vel_fast_; 00085 double linear_vel_slow_; 00086 00087 double cmd_roll_scale_; 00088 double cmd_pitch_scale_; 00089 double cmd_yaw_rate_scale_; 00090 double cmd_thrust_scale_; 00091 00092 // **** state variables 00093 00094 bool estop_btn_pressed_; 00095 bool takeoff_btn_pressed_; 00096 bool land_btn_pressed_; 00097 bool engage_btn_pressed_; 00098 bool pos_hold_btn_pressed_; 00099 bool vel_hold_btn_pressed_; 00100 00101 ros::Time last_joy_event_; 00102 00103 geometry_msgs::Twist::Ptr cmd_vel_msg_; 00104 00105 // **** member functions 00106 00107 void initParams(); 00108 00109 void joyCallback (const sensor_msgs::JoyPtr& joy_msg); 00110 //void estopCallback(const ros::TimerEvent& event); 00111 00112 public: 00113 00114 QuadJoyTeleop(ros::NodeHandle nh, ros::NodeHandle nh_private); 00115 virtual ~QuadJoyTeleop(); 00116 00117 }; 00118 00119 #endif // QUAD_JOY_TELEOP_QUAD_JOY_TELEOP_H