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 00041 #ifndef JACO_KEY_TELEOP_H_ 00042 #define JACO_KEY_TELEOP_H_ 00043 00044 #include <ros/ros.h> 00045 #include <boost/thread/mutex.hpp> 00046 #include <boost/thread/thread.hpp> 00047 #include <wpi_jaco_msgs/AngularCommand.h> 00048 #include <wpi_jaco_msgs/CartesianCommand.h> 00049 #include <signal.h> 00050 #include <stdio.h> 00051 #include <termios.h> 00052 00053 //Keycodes 00054 #define KEYCODE_W 0x77 00055 #define KEYCODE_A 0x61 00056 #define KEYCODE_S 0x73 00057 #define KEYCODE_D 0x64 00058 #define KEYCODE_Q 0x71 00059 #define KEYCODE_E 0x65 00060 #define KEYCODE_R 0x72 00061 #define KEYCODE_F 0x66 00062 #define KEYCODE_RIGHT 0x43 00063 #define KEYCODE_LEFT 0x44 00064 #define KEYCODE_UP 0x41 00065 #define KEYCODE_DOWN 0x42 00066 #define KEYCODE_1 0x31 00067 #define KEYCODE_2 0x32 00068 #define KEYCODE_3 0x33 00069 #define KEYCODE_H 0x68 00070 00071 //Control modes 00072 #define ARM_CONTROL 0 00073 #define FINGER_CONTROL 1 00074 00080 #define MAX_TRANS_VEL .175 00081 00086 #define MAX_FINGER_VEL 30 00087 00093 #define MAX_ANG_VEL 1.047 00094 00102 class jaco_key_teleop 00103 { 00104 public: 00112 jaco_key_teleop(); 00113 00117 void loop(); 00118 00122 void watchdog(); 00123 00124 private: 00125 00126 bool loadParameters(const ros::NodeHandle n); 00127 00133 void displayHelp(); 00134 00135 ros::NodeHandle nh_; 00136 ros::Time first_publish_, last_publish_; 00138 boost::mutex publish_mutex_; 00140 ros::Publisher angular_cmd; 00141 ros::Publisher cartesian_cmd; 00143 int mode; 00144 double linear_throttle_factor; 00145 double angular_throttle_factor; 00146 double finger_throttle_factor; 00147 std::string arm_name_; 00148 std::string topic_prefix_; 00149 }; 00150 00156 void shutdown(int sig); 00157 00165 int main(int argc, char **argv); 00166 00167 #endif