$search
00001 #!/usr/bin/env python 00002 00003 """ 00004 dynamixel_joint_state_publisher.py - Version 1.0 2010-12-28 00005 00006 Publish the dynamixel_controller joint states on the /joint_states topic 00007 00008 Created for the Pi Robot Project: http://www.pirobot.org 00009 Copyright (c) 2010 Patrick Goebel. All rights reserved. 00010 00011 This program is free software; you can redistribute it and/or modify 00012 it under the terms of the GNU General Public License as published by 00013 the Free Software Foundation; either version 2 of the License, or 00014 (at your option) any later version. 00015 00016 This program is distributed in the hope that it will be useful, 00017 but WITHOUT ANY WARRANTY; without even the implied warranty of 00018 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00019 GNU General Public License for more details at: 00020 00021 http://www.gnu.org/licenses/gpl.html 00022 """ 00023 00024 import roslib; roslib.load_manifest('pi_head_tracking_3d_part2') 00025 import rospy 00026 00027 from sensor_msgs.msg import JointState as JointStatePR2 00028 from dynamixel_msgs.msg import JointState as JointStateDynamixel 00029 00030 class JointStateMessage(): 00031 def __init__(self, name, position, velocity, effort): 00032 self.name = name 00033 self.position = position 00034 self.velocity = velocity 00035 self.effort = effort 00036 00037 class JointStatePublisher(): 00038 def __init__(self): 00039 rospy.init_node('dynamixel_joint_state_publisher', anonymous=True) 00040 00041 rate = rospy.get_param('~rate', 20) 00042 r = rospy.Rate(rate) 00043 00044 dynamixels = rospy.get_param('dynamixels', '') 00045 00046 self.joints = list() 00047 00048 self.servos = list() 00049 self.controllers = list() 00050 self.joint_states = dict({}) 00051 00052 for joint in sorted(dynamixels): 00053 controller = joint.replace("_joint", "") + "_controller" 00054 self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0) 00055 self.controllers.append(controller) 00056 00057 # Start controller state subscribers 00058 [rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers] 00059 00060 # Start publisher 00061 self.joint_states_pub = rospy.Publisher('/joint_states', JointStatePR2) 00062 00063 rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz") 00064 00065 while not rospy.is_shutdown(): 00066 self.publish_joint_states() 00067 r.sleep() 00068 00069 def controller_state_handler(self, msg): 00070 js = JointStateMessage(msg.name, msg.current_pos, msg.velocity, msg.load) 00071 self.joint_states[msg.name] = js 00072 00073 def publish_joint_states(self): 00074 # Construct message & publish joint states 00075 msg = JointStatePR2() 00076 msg.name = [] 00077 msg.position = [] 00078 msg.velocity = [] 00079 msg.effort = [] 00080 00081 for joint in self.joint_states.values(): 00082 msg.name.append(joint.name) 00083 msg.position.append(joint.position) 00084 msg.velocity.append(joint.velocity) 00085 msg.effort.append(joint.effort) 00086 00087 msg.header.stamp = rospy.Time.now() 00088 self.joint_states_pub.publish(msg) 00089 00090 if __name__ == '__main__': 00091 try: 00092 s = JointStatePublisher() 00093 rospy.spin() 00094 except rospy.ROSInterruptException: pass 00095