Go to the documentation of this file.00001
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
00049 parameters = rospy.get_param_names()
00050
00051 for parameter in parameters:
00052
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
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
00067 [rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
00068
00069
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
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