00001 /* 00002 * Copyright (c) 2011, Willow Garage, Inc. 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 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00043 #ifndef CARL_KEY_TELEOP_H_ 00044 #define CARL_KEY_TELEOP_H_ 00045 00046 #include <ros/ros.h> 00047 #include <boost/thread/mutex.hpp> 00048 #include <boost/thread/thread.hpp> 00049 #include <sensor_msgs/Joy.h> 00050 #include <signal.h> 00051 #include <termios.h> 00052 #include <wpi_jaco_msgs/AngularCommand.h> 00053 #include <wpi_jaco_msgs/CartesianCommand.h> 00054 00055 //Keycodes 00056 #define KEYCODE_W 0x77 00057 #define KEYCODE_A 0x61 00058 #define KEYCODE_S 0x73 00059 #define KEYCODE_D 0x64 00060 #define KEYCODE_Q 0x71 00061 #define KEYCODE_E 0x65 00062 #define KEYCODE_R 0x72 00063 #define KEYCODE_F 0x66 00064 #define KEYCODE_RIGHT 0x43 00065 #define KEYCODE_LEFT 0x44 00066 #define KEYCODE_UP 0x41 00067 #define KEYCODE_DOWN 0x42 00068 #define KEYCODE_1 0x31 00069 #define KEYCODE_2 0x32 00070 #define KEYCODE_3 0x33 00071 #define KEYCODE_H 0x68 00072 00073 //Control modes 00074 #define ARM_CONTROL 0 00075 #define FINGER_CONTROL 1 00076 #define BASE_CONTROL 2 00077 00078 //Arm limits 00084 #define MAX_TRANS_VEL_ARM .175 00085 00091 #define MAX_ANG_VEL_ARM 1.047 00092 00097 #define MAX_FINGER_VEL 30 00098 00104 #define MAX_TRANS_VEL_BASE .8 00105 00111 #define MAX_ANG_VEL_BASE 1.2 00112 00113 class carl_key_teleop 00114 { 00115 public: 00120 carl_key_teleop(); 00121 00125 void loop(); 00126 00130 void watchdog(); 00131 00132 private: 00138 void displayHelp(); 00139 00145 void publish(double, double); 00146 00147 ros::NodeHandle nh_; 00148 ros::Time first_publish_, last_publish_; 00150 boost::mutex publish_mutex_; 00152 ros::Publisher angular_cmd; 00153 ros::Publisher cartesian_cmd; 00154 ros::Publisher vel_pub_; 00156 int mode; 00157 double linear_throttle_factor_base; 00158 double angular_throttle_factor_base; 00159 double linear_throttle_factor_arm; 00160 double angular_throttle_factor_arm; 00161 double finger_throttle_factor; 00162 }; 00163 00169 void shutdown(int sig); 00170 00178 int main(int argc, char **argv); 00179 00180 #endif