00001 /* 00002 * 00003 * Copyright (C) 2015 University of Osnabrück, Germany 00004 * 00005 * This program is free software; you can redistribute it and/or 00006 * modify it under the terms of the GNU General Public License 00007 * as published by the Free Software Foundation; either version 2 00008 * of the License, or (at your option) any later version. 00009 * 00010 * This program is distributed in the hope that it will be useful, 00011 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00012 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00013 * GNU General Public License for more details. 00014 * 00015 * You should have received a copy of the GNU General Public License 00016 * along with this program; if not, write to the Free Software 00017 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. 00018 * 00019 * uos_rotunit_teleop_ps3joy.h 00020 * 00021 * Created on: 05.03.2015 00022 * Author: Sebastian Pütz <spuetz@uos.de> 00023 */ 00024 00025 #ifndef UOS_ROTUNIT_TELEOP_PS3JOY_H 00026 #define UOS_ROTUNIT_TELEOP_PS3JOY_H 00027 00028 #include <ros/ros.h> 00029 #include <geometry_msgs/Twist.h> 00030 #include <sensor_msgs/Joy.h> 00031 #include <algorithm> 00032 00033 #include <actionlib/client/simple_action_client.h> 00034 #include <actionlib/client/terminal_state.h> 00035 #include <uos_rotunit_snapshotter/RotunitSnapshotAction.h> 00036 #include <uos_rotunit_driver/RotVelSrv.h> 00037 00038 #define PS3_BUTTON_SELECT 0 00039 #define PS3_BUTTON_STICK_LEFT 1 00040 #define PS3_BUTTON_STICK_RIGHT 2 00041 #define PS3_BUTTON_START 3 00042 #define PS3_BUTTON_CROSS_UP 4 00043 #define PS3_BUTTON_CROSS_RIGHT 5 00044 #define PS3_BUTTON_CROSS_DOWN 6 00045 #define PS3_BUTTON_CROSS_LEFT 7 00046 #define PS3_BUTTON_REAR_LEFT_2 8 00047 #define PS3_BUTTON_REAR_RIGHT_2 9 00048 #define PS3_BUTTON_REAR_LEFT_1 10 00049 #define PS3_BUTTON_REAR_RIGHT_1 11 00050 #define PS3_BUTTON_ACTION_TRIANGLE 12 00051 #define PS3_BUTTON_ACTION_CIRCLE 13 00052 #define PS3_BUTTON_ACTION_CROSS 14 00053 #define PS3_BUTTON_ACTION_SQUARE 15 00054 #define PS3_BUTTON_PAIRING 16 00055 00056 #define PS3_AXIS_STICK_LEFT_LEFTWARDS 0 00057 #define PS3_AXIS_STICK_LEFT_UPWARDS 1 00058 #define PS3_AXIS_STICK_RIGHT_LEFTWARDS 2 00059 #define PS3_AXIS_STICK_RIGHT_UPWARDS 3 00060 #define PS3_AXIS_BUTTON_CROSS_UP 4 00061 #define PS3_AXIS_BUTTON_CROSS_RIGHT 5 00062 #define PS3_AXIS_BUTTON_CROSS_DOWN 6 00063 #define PS3_AXIS_BUTTON_CROSS_LEFT 7 00064 #define PS3_AXIS_BUTTON_REAR_LEFT_2 8 00065 #define PS3_AXIS_BUTTON_REAR_RIGHT_2 9 00066 #define PS3_AXIS_BUTTON_REAR_LEFT_1 10 00067 #define PS3_AXIS_BUTTON_REAR_RIGHT_1 11 00068 #define PS3_AXIS_BUTTON_ACTION_TRIANGLE 12 00069 #define PS3_AXIS_BUTTON_ACTION_CIRCLE 13 00070 #define PS3_AXIS_BUTTON_ACTION_CROSS 14 00071 #define PS3_AXIS_BUTTON_ACTION_SQUARE 15 00072 #define PS3_AXIS_ACCELEROMETER_LEFT 16 00073 #define PS3_AXIS_ACCELEROMETER_FORWARD 17 00074 #define PS3_AXIS_ACCELEROMETER_UP 18 00075 #define PS3_AXIS_GYRO_YAW 19 00076 00077 class RotunitTeleopPS3Joy{ 00078 00079 public: 00080 RotunitTeleopPS3Joy(ros::NodeHandle &nh); 00081 00082 private: 00083 void PS3Callback(const sensor_msgs::Joy::ConstPtr &joy); 00084 00085 ros::NodeHandle nh_; 00086 ros::Subscriber joy_sub_; 00087 ros::Publisher vel_pub_; 00088 ros::ServiceClient rot_vel_client_; 00089 00090 double acc_; 00091 double vel_; 00092 double scan_vel_; 00093 actionlib::SimpleActionClient<uos_rotunit_snapshotter::RotunitSnapshotAction> ac; 00094 }; 00095 00096 00097 #endif /* uos_rotunit_teleop_ps3joy.h */