00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2014, Alexander Buchegger 00006 * Institute for Software Technology, 00007 * Graz University of Technology 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of Graz University of Technology nor the names of 00021 * its contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 *********************************************************************/ 00038 #ifndef TEDUSAR_CARTESIAN_ARM_TELEOP__CARTESIAN_ARM_TELEOP_NODE_H_ 00039 #define TEDUSAR_CARTESIAN_ARM_TELEOP__CARTESIAN_ARM_TELEOP_NODE_H_ 00040 00041 #include <string> 00042 #include <boost/scoped_ptr.hpp> 00043 #include <ros/node_handle.h> 00044 #include <ros/timer.h> 00045 #include <ros/subscriber.h> 00046 #include <ros/publisher.h> 00047 #include <sensor_msgs/Joy.h> 00048 #include <geometry_msgs/TwistStamped.h> 00049 #include <control_msgs/GripperCommandAction.h> 00050 #include <actionlib/client/simple_action_client.h> 00051 #include <std_msgs/Empty.h> 00052 00053 00054 namespace tedusar_cartesian_arm_teleop 00055 { 00056 00057 class CartesianArmTeleopNode 00058 { 00059 public: 00060 CartesianArmTeleopNode(); 00061 00062 void init(); 00063 00064 private: 00065 struct Parameters 00066 { 00067 double publication_rate_; 00068 std::string frame_id_; 00069 00070 double std_translation_vel_; 00071 double std_rotation_vel_; 00072 double turbo_factor_; 00073 00074 bool have_gripper_; 00075 double gripper_grasp_position_; 00076 double gripper_grasp_effort_; 00077 double gripper_release_position_; 00078 double gripper_release_effort_; 00079 00080 std::string controller_; 00081 00082 int button_translation_deadman_switch_; 00083 int button_rotation_deadman_switch_; 00084 int button_turbo_; 00085 int button_gripper_grasp_; 00086 int button_gripper_release_; 00087 int button_grasp_done_; 00088 00089 int axis_translation_x_; 00090 int axis_translation_y_; 00091 int axis_translation_z_; 00092 int axis_rotation_x_; 00093 int axis_rotation_y_; 00094 int axis_rotation_z_; 00095 }; 00096 00097 typedef actionlib::SimpleActionClient<control_msgs::GripperCommandAction> GripperCommandActionClient; 00098 00099 void loadParameters(); 00100 00101 void cmdGeneratorTimerCB(const ros::TimerEvent & e); 00102 00103 void joyCB(const sensor_msgs::Joy::ConstPtr & joy); 00104 00105 void sendCmdVel(); 00106 void sendGripperCommand(double position, double max_effort); 00107 00108 void sendGraspDone(); 00109 00110 template <typename T> static void getRequiredParameter(ros::NodeHandle & private_nh, const std::string & key, T & value); 00111 template <typename T> static void getOptionalParameter(ros::NodeHandle & private_nh, const std::string & key, T & value, T default_value); 00112 00113 static double limit(double value, double min, double max); 00114 00115 ros::NodeHandle nh_; 00116 Parameters parameters_; 00117 bool repeat_vel_commands_; 00118 00119 ros::Subscriber joy_sub_; 00120 ros::Timer cmd_generator_timer_; 00121 ros::Publisher cmd_vel_pub_; 00122 boost::scoped_ptr<GripperCommandActionClient> gripper_command_ac_; 00123 00124 ros::Publisher grasping_done_pub_; 00125 00126 geometry_msgs::TwistStamped cmd_vel_; 00127 }; 00128 00129 } 00130 00131 #endif // TEDUSAR_CARTESIAN_ARM_TELEOP__CARTESIAN_ARM_TELEOP_NODE_H_