Go to the documentation of this file.00001
00002 """
00003 @file widowx_gripper.py
00004
00005
00006 Subscribes:
00007 -
00008
00009 Publishes:
00010 -
00011
00012 Actions:
00013
00014
00015 @author: Robotnik Automation
00016
00017 Software License Agreement (BSD License)
00018
00019 Copyright (c) 2015 Robotnik Automation SLL. All Rights Reserved.
00020
00021 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00022
00023 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
00024
00025 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
00026 in the documentation and/or other materials provided with the distribution.
00027
00028 3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.
00029
00030 THIS SOFTWARE IS PROVIDED BY ROBOTNIK AUTOMATION SLL "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
00031 AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
00032 OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
00033 AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
00034 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00035 """
00036
00037 import roslib; roslib.load_manifest('widowx_arm_controller')
00038 import rospy
00039 import actionlib
00040 from control_msgs.msg import *
00041 from std_msgs.msg import *
00042 from sensor_msgs.msg import JointState
00043
00044
00045
00046 class WidowxGripper:
00047 """
00048 Class to communicate with the dinamyxel controllers of the arm
00049 """
00050 def __init__(self):
00051
00052 self.distance = 0.0;
00053
00054 try:
00055 self.name = rospy.get_param('~name', default = 'widowx')
00056 self.revolute_command = rospy.get_param('~revolute_command', default = '/gripper_revolute_joint/command')
00057 self.joint_name = rospy.get_param('~joint_name', default = 'gripper_prismatic_joint_1')
00058 self.prismatic_command = rospy.get_param('~prismatic_command', default = '/gripper_prismatic_joint/command')
00059
00060 self.gripper_revolute_joint_state_subscriber = rospy.Subscriber('/joint_states', JointState, self.jointStateCb)
00061 self.gripper_prismatic_joint_subscriber = rospy.Subscriber(self.prismatic_command, Float64, self.gripperPrismaticJointCommandCb)
00062 self.gripper_prismatic_joint_state_publisher = rospy.Publisher('/joint_states', JointState, queue_size=10)
00063 self.gripper_revolute_joint_publisher = rospy.Publisher(self.revolute_command, Float64, queue_size=10)
00064
00065
00066 self.desired_freq = rospy.get_param('~desired_freq', default = 10.0)
00067
00068 except rospy.ROSException, e:
00069 rospy.logerr('%s: error getting params %s'%(rospy.get_name(), e))
00070 exit()
00071
00072 def jointStateCb(self, msg):
00073
00074 if 'gripper_revolute_joint' in msg.name:
00075 index = msg.name.index('gripper_revolute_joint')
00076 position = msg.position[index]
00077 velocity = msg.velocity[index]
00078
00079 self.distance = (-0.01154 * position) + 0.03
00080
00081 def gripperPrismaticJointCommandCb(self, msg):
00082 """
00083 Receives joint command and send it to the controller
00084 """
00085
00086
00087 gripper_goal = Float64()
00088
00089 if(msg.data >= 0 and msg.data <= 0.03):
00090 self.distance = msg.data;
00091 rad = (-86.67 * self.distance) + 2.6
00092 gripper_goal.data = rad
00093 self.gripper_revolute_joint_publisher.publish(gripper_goal)
00094
00095
00096 def controlLoop(self):
00097 """
00098 Runs the control loop
00099 """
00100 joint_state_gripper = JointState()
00101
00102 joint_state_gripper.header = Header()
00103 joint_state_gripper.header.stamp = rospy.Time.now()
00104 joint_state_gripper.name = [self.joint_name]
00105 joint_state_gripper.position = [self.distance]
00106 joint_state_gripper.velocity = [0.0]
00107 joint_state_gripper.effort = [0.0]
00108
00109 self.gripper_prismatic_joint_state_publisher.publish(joint_state_gripper)
00110
00111 t_sleep = 1.0/self.desired_freq
00112
00113 while not rospy.is_shutdown():
00114 joint_state_gripper.header.stamp = rospy.Time.now()
00115 joint_state_gripper.position = [self.distance]
00116
00117 self.gripper_prismatic_joint_state_publisher.publish(joint_state_gripper)
00118 rospy.sleep(t_sleep)
00119
00120 def start(self):
00121 """
00122 Starts the action server and runs spin
00123 """
00124 try:
00125 self.controlLoop()
00126 except rospy.ROSInterruptException:
00127 rospy.loginfo('%s: Bye!'%rospy.get_name())
00128
00129 def main():
00130
00131 rospy.init_node('widowx_gripper_node')
00132
00133 widowx_node = WidowxGripper()
00134
00135 widowx_node.start()
00136
00137
00138 if __name__=='__main__':
00139 main()
00140 exit()
00141