00001
00002
00003 """
00004 @file phantomx_reactor_parallel_motor_joints.py
00005
00006
00007 Subscribes:
00008 - joint_command
00009
00010 Publishes:
00011 - joint_states
00012
00013 @author: Robotnik Automation
00014
00015 Software License Agreement (BSD License)
00016
00017 Copyright (c) 2015 Robotnik Automation SLL. All Rights Reserved.
00018
00019 Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00020
00021 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
00022
00023 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer
00024 in the documentation and/or other materials provided with the distribution.
00025
00026 3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.
00027
00028 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
00029 AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY,
00030 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
00031 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,
00032 EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00033 """
00034
00035 import roslib; roslib.load_manifest('phantomx_reactor_arm_controller')
00036 import rospy
00037 import actionlib
00038 from control_msgs.msg import *
00039 from std_msgs.msg import *
00040 from sensor_msgs.msg import JointState
00041
00042
00043
00044 class PhantomXReactor:
00045 """
00046 Class to communicate with the dinamyxel controllers of the arm
00047 """
00048 def __init__(self):
00049
00050 try:
00051 self.name = rospy.get_param('~name', default = 'phantomx_reactor')
00052 self.j1 = rospy.get_param('~j1', default = 'shoulder_yaw_joint')
00053 self.j2 = rospy.get_param('~j2', default = 'shoulder_pitch_joint')
00054 self.j3 = rospy.get_param('~j3', default = 'elbow_pitch_joint')
00055 self.j4 = rospy.get_param('~j4', default = 'wrist_pitch_joint')
00056 self.j5 = rospy.get_param('~j5', default = 'wrist_roll_joint')
00057 self.j6 = rospy.get_param('~j6', default = 'gripper_revolute_joint')
00058 self.controller_1 = rospy.get_param('~controller_1', default = '/shoulder_yaw_joint/command')
00059 self.controller_2 = rospy.get_param('~controller_2', default = '/shoulder_pitch_joint/command')
00060 self.controller_3 = rospy.get_param('~controller_3', default = '/shoulder_pitch_mimic_joint/command')
00061 self.controller_4 = rospy.get_param('~controller_4', default = '/elbow_pitch_joint/command')
00062 self.controller_5 = rospy.get_param('~controller_5', default = '/elbow_pitch_mimic_joint/command')
00063 self.controller_6 = rospy.get_param('~controller_6', default = '/wrist_pitch_joint/command')
00064 self.controller_7 = rospy.get_param('~controller_7', default = '/wrist_roll_joint/command')
00065 self.controller_8 = rospy.get_param('~controller_8', default = '/gripper_revolute_joint/command')
00066
00067
00068
00069 self.controller_1_publisher = rospy.Publisher(self.controller_1, Float64, queue_size = 10)
00070 self.controller_2_publisher = rospy.Publisher(self.controller_2, Float64, queue_size = 10)
00071 self.controller_3_publisher = rospy.Publisher(self.controller_3, Float64, queue_size = 10)
00072 self.controller_4_publisher = rospy.Publisher(self.controller_4, Float64, queue_size = 10)
00073 self.controller_5_publisher = rospy.Publisher(self.controller_5, Float64, queue_size = 10)
00074 self.controller_6_publisher = rospy.Publisher(self.controller_6, Float64, queue_size = 10)
00075 self.controller_7_publisher = rospy.Publisher(self.controller_7, Float64, queue_size = 10)
00076 self.controller_8_publisher = rospy.Publisher(self.controller_8, Float64, queue_size = 10)
00077
00078
00079 self.joint_1_command = rospy.Subscriber('~'+self.j1+"/command", Float64, self.joint1CommandCb)
00080 self.joint_2_command = rospy.Subscriber('~'+self.j2+"/command", Float64, self.joint2CommandCb)
00081 self.joint_3_command = rospy.Subscriber('~'+self.j3+"/command", Float64, self.joint3CommandCb)
00082 self.joint_4_command = rospy.Subscriber('~'+self.j4+"/command", Float64, self.joint4CommandCb)
00083 self.joint_5_command = rospy.Subscriber('~'+self.j5+"/command", Float64, self.joint5CommandCb)
00084 self.joint_6_command = rospy.Subscriber('~'+self.j6+"/command", Float64, self.joint6CommandCb)
00085
00086
00087 self.joint_command = rospy.Subscriber('~joint_command', JointState, self.jointCommandCb)
00088
00089 self.desired_freq = rospy.get_param('~desired_freq', default = 10.0)
00090
00091 except rospy.ROSException, e:
00092 rospy.logerr('%s: error getting params %s'%(rospy.get_name(), e))
00093 exit()
00094
00095
00096 def jointCommandCb(self, msg):
00097 """
00098 Receives joint command and send it to the controller
00099 """
00100 if len(msg.name) != len(msg.position):
00101 rospy.logerr('%s:jointCommandCb: length of joint names and position has to be equal'%(rospy.get_name()))
00102 return
00103 for i in range(len(msg.name)):
00104
00105 self.setJointPosition(msg.name[i], msg.position[i])
00106
00107 def joint1CommandCb(self, msg):
00108 """
00109 Receives joint 1 command and send it to the controller
00110 """
00111 self.setJointPosition(self.j1, msg.data)
00112
00113 def joint2CommandCb(self, msg):
00114 """
00115 Receives joint 2 command and send it to the controller
00116 """
00117 self.setJointPosition(self.j2, msg.data)
00118
00119 def joint3CommandCb(self, msg):
00120 """
00121 Receives joint 3 command and send it to the controller
00122 """
00123 self.setJointPosition(self.j3, msg.data)
00124
00125
00126 def joint4CommandCb(self, msg):
00127 """
00128 Receives joint 4 command and send it to the controller
00129 """
00130 self.setJointPosition(self.j4, msg.data)
00131
00132
00133 def joint5CommandCb(self, msg):
00134 """
00135 Receives joint 5 command and send it to the controller
00136 """
00137 self.setJointPosition(self.j5, msg.data)
00138
00139
00140 def joint6CommandCb(self, msg):
00141 """
00142 Receives joint 6 command and send it to the controller
00143 """
00144 self.setJointPosition(self.j6, msg.data)
00145
00146
00147 def controlLoop(self):
00148 """
00149 Runs the control loop
00150 """
00151 t_sleep = 1.0/self.desired_freq
00152
00153 while not rospy.is_shutdown():
00154 rospy.sleep(t_sleep)
00155
00156
00157 def setJointPosition(self, joint, position):
00158 """
00159 Sends the command to the controller to set the desired joint position
00160 """
00161 msg = Float64()
00162 msg.data = position
00163
00164 if joint == self.j1:
00165 self.controller_1_publisher.publish(msg)
00166 elif joint == self.j2:
00167 self.controller_2_publisher.publish(msg)
00168 msg.data = -position
00169 self.controller_3_publisher.publish(msg)
00170 elif joint == self.j3:
00171 self.controller_4_publisher.publish(msg)
00172 msg.data = -position
00173 self.controller_5_publisher.publish(msg)
00174 elif joint == self.j4:
00175 self.controller_6_publisher.publish(msg)
00176 elif joint == self.j5:
00177 self.controller_7_publisher.publish(msg)
00178 elif joint == self.j6:
00179 self.controller_8_publisher.publish(msg)
00180
00181
00182 def start(self):
00183 """
00184 Starts the control loop and runs spin
00185 """
00186 try:
00187 self.controlLoop()
00188 except rospy.ROSInterruptException:
00189 rospy.loginfo('%s: Bye!'%rospy.get_name())
00190
00191
00192 def main():
00193
00194 rospy.init_node('robotnik_phantomx_reactor')
00195
00196 phantomx_reactor_node = PhantomXReactor()
00197
00198 phantomx_reactor_node.start()
00199
00200
00201
00202 if __name__=='__main__':
00203 main()
00204 exit()
00205