00001 /* 00002 * UOS-ROS packages - Robot Operating System code by the University of Osnabrück 00003 * Copyright (C) 2011 University of Osnabrück 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 * katana_teleop_key.h 00020 * 00021 * Created on: 13.04.2011 00022 * Author: Henning Deeken <hdeeken@uos.de> 00023 */ 00024 #ifndef KATANA_TELEOP_KEY_H__ 00025 #define KATANA_TELEOP_KEY_H__ 00026 00027 00028 #include <termios.h> 00029 #include <signal.h> 00030 #include <math.h> 00031 #include <stdio.h> 00032 #include <stdlib.h> 00033 00034 #include <ros/ros.h> 00035 #include <sensor_msgs/JointState.h> 00036 #include <katana_msgs/JointMovementAction.h> 00037 #include <control_msgs/GripperCommandAction.h> 00038 00039 #include <actionlib/server/simple_action_server.h> 00040 #include <actionlib/client/simple_action_client.h> 00041 00042 typedef actionlib::SimpleActionClient<katana_msgs::JointMovementAction> JMAC; 00043 typedef control_msgs::GripperCommandGoal GCG; 00044 00045 #define KEYCODE_A 0x61 00046 #define KEYCODE_D 0x64 00047 #define KEYCODE_S 0x73 00048 #define KEYCODE_W 0x77 00049 00050 #define KEYCODE_R 0x72 00051 #define KEYCODE_Q 0x71 00052 #define KEYCODE_I 0x69 00053 00054 #define KEYCODE_O 0x6F 00055 #define KEYCODE_C 0x63 00056 00057 #define KEYCODE_PLUS 0x2B 00058 #define KEYCODE_NUMBER 0x23 00059 #define KEYCODE_POINT 0x2E 00060 #define KEYCODE_COMMA 0x2C 00061 00062 #define KEYCODE_0 0x30 00063 #define KEYCODE_1 0x31 00064 #define KEYCODE_2 0x32 00065 #define KEYCODE_3 0x33 00066 #define KEYCODE_4 0x34 00067 #define KEYCODE_5 0x35 00068 #define KEYCODE_6 0x36 00069 #define KEYCODE_7 0x37 00070 #define KEYCODE_8 0x38 00071 #define KEYCODE_9 0x39 00072 00073 #define GRASP 1 00074 #define RELEASE 2 00075 00076 struct termios cooked, raw; 00077 int kfd = 0; 00078 bool got_joint_states_; 00079 00080 00081 namespace katana{ 00082 00083 class KatanaTeleopKey 00084 { 00085 00086 public: 00087 KatanaTeleopKey(); 00088 00089 void jointStateCallback(const sensor_msgs::JointState::ConstPtr& js); 00090 void keyboardLoop(); 00091 00092 ~KatanaTeleopKey() { } 00093 00094 private: 00095 bool matchJointGoalRequest(double increment); 00096 void giveInfo(); 00097 bool send_gripper_action(int32_t goal_type); 00098 00099 size_t jointIndex; 00100 double increment; 00101 double increment_step; 00102 double increment_step_scaling; 00103 00104 std::vector<std::string> joint_names_; 00105 std::vector<std::string> gripper_joint_names_; 00106 std::vector<std::string> combined_joints_; 00107 00108 sensor_msgs::JointState movement_goal_; 00109 sensor_msgs::JointState current_pose_; 00110 sensor_msgs::JointState initial_pose_; 00111 00112 JMAC action_client; 00113 actionlib::SimpleActionClient<control_msgs::GripperCommandAction> gripper_; 00114 ros::Subscriber js_sub_; 00115 }; 00116 } 00117 00118 #endif /* KATANA_TELEOP_KEY_H__ */