Go to the documentation of this file.00001
00002
00003 """
00004 dynamixel_joint_state_publisher.py - Version 1.1 2013-12-20
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 rospy
00025 from sensor_msgs.msg import JointState as JointStatePR2
00026 from dynamixel_msgs.msg import JointState as JointStateDynamixel
00027
00028 class JointStateMessage():
00029 def __init__(self, name, position, velocity, effort):
00030 self.name = name
00031 self.position = position
00032 self.velocity = velocity
00033 self.effort = effort
00034
00035 class JointStatePublisher():
00036 def __init__(self):
00037 rospy.init_node('dynamixel_joint_state_publisher', anonymous=True)
00038
00039 rate = rospy.get_param('~rate', 20)
00040 r = rospy.Rate(rate)
00041
00042
00043
00044 namespace = rospy.get_namespace()
00045 self.joints = rospy.get_param(namespace + '/joints', '')
00046
00047 self.servos = list()
00048 self.controllers = list()
00049 self.joint_states = dict({})
00050
00051 for controller in sorted(self.joints):
00052 self.joint_states[controller] = JointStateMessage(controller, 0.0, 0.0, 0.0)
00053 self.controllers.append(controller)
00054
00055
00056 [rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
00057
00058
00059 self.joint_states_pub = rospy.Publisher('/joint_states', JointStatePR2, queue_size=5)
00060
00061 rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz")
00062
00063 while not rospy.is_shutdown():
00064 self.publish_joint_states()
00065 r.sleep()
00066
00067 def controller_state_handler(self, msg):
00068 js = JointStateMessage(msg.name, msg.current_pos, msg.velocity, msg.load)
00069 self.joint_states[msg.name] = js
00070
00071 def publish_joint_states(self):
00072
00073 msg = JointStatePR2()
00074 msg.name = []
00075 msg.position = []
00076 msg.velocity = []
00077 msg.effort = []
00078
00079 for joint in self.joint_states.values():
00080 msg.name.append(joint.name)
00081 msg.position.append(joint.position)
00082 msg.velocity.append(joint.velocity)
00083 msg.effort.append(joint.effort)
00084
00085 msg.header.stamp = rospy.Time.now()
00086 msg.header.frame_id = 'base_link'
00087 self.joint_states_pub.publish(msg)
00088
00089 if __name__ == '__main__':
00090 try:
00091 s = JointStatePublisher()
00092 rospy.spin()
00093 except rospy.ROSInterruptException: pass
00094