widowx_gripper.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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                 #rospy.loginfo('%s: info getting params'%rospy.get_name())
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         


widowx_arm_controller
Author(s):
autogenerated on Wed Jul 10 2019 03:40:28