komodo_arm: states_converter.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 import roslib; roslib.load_manifest('komodo_arm')
00003 import rospy
00004 from dynamixel_msgs.msg import JointState as dxl_JointState
00005 from sensor_msgs.msg import JointState
00006 
00007 def br_callback(data):       
00008     global msg
00009     msg.name[0]=data.name
00010     msg.position[0]=data.current_pos
00011     msg.velocity[0]=data.velocity
00012     msg.effort[0]=data.load
00013 
00014 def sh_callback(data):       
00015     global msg
00016     msg.name[1]=data.name
00017     msg.position[1]=data.current_pos
00018     msg.velocity[1]=data.velocity
00019     msg.effort[1]=data.load
00020 
00021 def e1_callback(data):       
00022     global msg
00023     msg.name[2]=data.name
00024     msg.position[2]=data.current_pos
00025     msg.velocity[2]=data.velocity
00026     msg.effort[2]=data.load
00027 
00028 def e2_callback(data):       
00029     global msg
00030     msg.name[3]=data.name
00031     msg.position[3]=data.current_pos
00032     msg.velocity[3]=data.velocity
00033     msg.effort[3]=data.load
00034 
00035 def wr_callback(data):       
00036     global msg
00037     msg.name[4]=data.name
00038     msg.position[4]=data.current_pos
00039     msg.velocity[4]=data.velocity
00040     msg.effort[4]=data.load
00041 
00042 def lf_callback(data):       
00043     global msg
00044     msg.name[5]=data.name
00045     msg.position[5]=data.current_pos
00046     msg.velocity[5]=data.velocity
00047     msg.effort[5]=data.load
00048 
00049 def rf_callback(data):       
00050     global msg
00051     msg.name[6]=data.name
00052     msg.position[6]=data.current_pos
00053     msg.velocity[6]=data.velocity
00054     msg.effort[6]=data.load
00055 
00056 
00057 
00058 def states_converter():
00059     global pub
00060     global msg
00061     msg = JointState()
00062     for i in range(7):
00063         msg.name.append("")
00064         msg.position.append(0.0)
00065         msg.velocity.append(0.0)
00066         msg.effort.append(0.0)
00067     rospy.init_node('states_converter', anonymous=True)
00068     rospy.Subscriber("/base_rotation_controller/state", dxl_JointState, br_callback)
00069     rospy.Subscriber("/shoulder_controller/state", dxl_JointState, sh_callback)
00070     rospy.Subscriber("/elbow1_controller/state", dxl_JointState, e1_callback)
00071     rospy.Subscriber("/elbow2_controller/state", dxl_JointState, e2_callback)
00072     rospy.Subscriber("/left_finger_controller/state", dxl_JointState, lf_callback)
00073     rospy.Subscriber("/right_finger_controller/state", dxl_JointState, rf_callback)
00074     rospy.Subscriber("/wrist_controller/state", dxl_JointState, wr_callback)
00075     pub = rospy.Publisher('joint_states', JointState)
00076     while not rospy.is_shutdown():
00077         #rospy.loginfo("running states_converter.py") 
00078         msg.header.stamp = rospy.Time.now()
00079         pub.publish(msg)
00080         rospy.sleep(0.1)
00081         #rospy.loginfo(msg)
00082 if __name__ == '__main__':
00083     #rospy.loginfo("Starting states_converter.py")
00084     states_converter()
00085 
00086