$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_part1') 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 dynamixel_namespace = rospy.get_namespace() 00042 00043 rate = rospy.get_param('~rate', 10) 00044 r = rospy.Rate(rate) 00045 00046 self.joints = list() 00047 00048 # Get all parameter names 00049 parameters = rospy.get_param_names() 00050 00051 for parameter in parameters: 00052 # Look for the parameters that name the joints. 00053 if parameter.find(dynamixel_namespace) != -1 and parameter.find("joint_name") != -1: 00054 self.joints.append(rospy.get_param(parameter)) 00055 00056 self.servos = list() 00057 self.controllers = list() 00058 self.joint_states = dict({}) 00059 00060 for joint in self.joints: 00061 # Remove "_joint" from the end of the joint name to get the controller names. 00062 servo = joint.split("_joint")[0] 00063 self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0) 00064 self.controllers.append(dynamixel_namespace + servo + "_controller") 00065 00066 # Start controller state subscribers 00067 [rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers] 00068 00069 # Start publisher 00070 self.joint_states_pub = rospy.Publisher('/joint_states', JointStatePR2) 00071 00072 rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz") 00073 00074 while not rospy.is_shutdown(): 00075 self.publish_joint_states() 00076 r.sleep() 00077 00078 def controller_state_handler(self, msg): 00079 js = JointStateMessage(msg.name, msg.current_pos, msg.velocity, msg.load) 00080 self.joint_states[msg.name] = js 00081 00082 def publish_joint_states(self): 00083 # Construct message & publish joint states 00084 msg = JointStatePR2() 00085 msg.name = [] 00086 msg.position = [] 00087 msg.velocity = [] 00088 msg.effort = [] 00089 00090 for joint in self.joint_states.values(): 00091 msg.name.append(joint.name) 00092 msg.position.append(joint.position) 00093 msg.velocity.append(joint.velocity) 00094 msg.effort.append(joint.effort) 00095 00096 msg.header.stamp = rospy.Time.now() 00097 self.joint_states_pub.publish(msg) 00098 00099 if __name__ == '__main__': 00100 try: 00101 s = JointStatePublisher() 00102 rospy.spin() 00103 except rospy.ROSInterruptException: pass 00104