pi_head_tracking_3d_part2: dynamixel_joint_state

Go to the documentation of this file.
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