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 <object_manipulation_msgs/GraspHandPostureExecutionAction.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 object_manipulation_msgs::GraspHandPostureExecutionGoal GHPEG; 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 struct termios cooked, raw; 00074 int kfd = 0; 00075 bool got_joint_states_; 00076 00077 00078 namespace katana{ 00079 00080 class KatanaTeleopKey 00081 { 00082 00083 public: 00084 KatanaTeleopKey(); 00085 00086 void jointStateCallback(const sensor_msgs::JointState::ConstPtr& js); 00087 void keyboardLoop(); 00088 00089 ~KatanaTeleopKey() { } 00090 00091 private: 00092 bool matchJointGoalRequest(double increment); 00093 void giveInfo(); 00094 bool send_gripper_action(int32_t goal_type); 00095 00096 size_t jointIndex; 00097 double increment; 00098 double increment_step; 00099 double increment_step_scaling; 00100 00101 std::vector<std::string> joint_names_; 00102 std::vector<std::string> gripper_joint_names_; 00103 std::vector<std::string> combined_joints_; 00104 00105 sensor_msgs::JointState movement_goal_; 00106 sensor_msgs::JointState current_pose_; 00107 sensor_msgs::JointState initial_pose_; 00108 00109 JMAC action_client; 00110 actionlib::SimpleActionClient<object_manipulation_msgs::GraspHandPostureExecutionAction> gripper_; 00111 ros::Subscriber js_sub_; 00112 }; 00113 } 00114 00115 #endif /* KATANA_TELEOP_KEY_H__ */
