00001 00015 #ifndef CARL_JOY_TELEOP_H_ 00016 #define CARL_JOY_TELEOP_H_ 00017 00018 #include <ros/ros.h> 00019 #include <actionlib/client/simple_action_client.h> 00020 #include <rail_manipulation_msgs/ArmAction.h> 00021 #include <sensor_msgs/Joy.h> 00022 #include <std_msgs/Float64.h> 00023 #include <std_srvs/Empty.h> 00024 #include <wpi_jaco_msgs/AngularCommand.h> 00025 #include <wpi_jaco_msgs/CartesianCommand.h> 00026 #include <wpi_jaco_msgs/EStop.h> 00027 00028 //Control modes 00029 #define ARM_CONTROL 0 00030 #define FINGER_CONTROL 1 00031 #define BASE_CONTROL 2 00032 #define SENSOR_CONTROL 3 00033 00034 //Joystick types 00035 #define ANALOG 0 //analog triggers 00036 #define DIGITAL 1 //digital triggers 00037 00038 //Arm Parameters 00044 #define MAX_TRANS_VEL_ARM .175 00045 00051 #define MAX_ANG_VEL_ARM 1.047 00052 00057 #define MAX_FINGER_VEL 30 00058 00059 //Base Parameters 00065 #define MAX_TRANS_VEL_BASE 1.0 00066 00072 #define MAX_ANG_VEL_BASE 1.0 00073 00079 #define NON_BOOST_THROTTLE 0.8 00080 00088 class carl_joy_teleop 00089 { 00090 public: 00095 carl_joy_teleop(); 00096 00100 void publish_velocity(); 00101 00102 private: 00108 void joy_cback(const sensor_msgs::Joy::ConstPtr& joy); 00109 00110 void displayHelp(int menuNumber); 00111 00112 ros::NodeHandle node; 00114 ros::Publisher cmd_vel; 00115 ros::Publisher angular_cmd; 00116 ros::Publisher cartesian_cmd; 00117 ros::Publisher cartesian_cmd2; 00118 ros::Publisher asus_servo_tilt_cmd; /*< velocity command to tilt the asus servo */ 00119 ros::Publisher creative_servo_pan_cmd; /*< velocity command to pan the creative servo */ 00120 ros::Subscriber joy_sub; 00122 ros::ServiceClient segment_client; 00123 ros::ServiceClient eStopClient; 00125 actionlib::SimpleActionClient<rail_manipulation_msgs::ArmAction> acArm; 00127 geometry_msgs::Twist twist; 00128 wpi_jaco_msgs::AngularCommand fingerCmd; 00129 wpi_jaco_msgs::CartesianCommand cartesianCmd; 00130 geometry_msgs::Twist cartesianCmd2; 00131 00132 int mode; 00133 int controllerType; 00134 int rightBumperPrev; 00135 int leftBumperPrev; 00136 int rightStickPrev; 00137 double linear_throttle_factor_base; 00138 double angular_throttle_factor_base; 00139 double linear_throttle_factor_arm; 00140 double angular_throttle_factor_arm; 00141 double finger_throttle_factor; 00142 bool stopMessageSentArm; 00143 bool stopMessageSentFinger; 00144 bool initLeftTrigger; 00145 bool initRightTrigger; 00146 bool calibrated; 00147 bool EStopEnabled; 00148 bool helpDisplayed; 00149 bool deadman; 00150 bool use_teleop_safety; 00151 }; 00152 00160 int main(int argc, char **argv); 00161 00162 #endif