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 import rospy
00024
00025 from sensor_msgs.msg import JointState as JointStatePR2
00026 from dynamixel_msgs.msg import JointState as JointStateDynamixel
00027 from dynamixel_controllers.srv import SetSpeed
00028 from dynamic_reconfigure.server import Server
00029 from jsk_tilt_laser.cfg import DynamixelTiltControllerConfig
00030
00031 class JointStateMessage():
00032 def __init__(self, name, position, velocity, effort):
00033 self.name = name
00034 self.position = position
00035 self.velocity = velocity
00036 self.effort = effort
00037
00038 class JointStatePublisher():
00039 def __init__(self):
00040 rospy.init_node('dynamixel_joint_state_publisher', anonymous=True)
00041 rospy.wait_for_service('tilt_controller/set_speed')
00042 self.srv = Server(DynamixelTiltControllerConfig, self.reconfigure_callback)
00043 dynamixel_namespace = rospy.get_namespace()
00044 rate = rospy.get_param('~rate', 10)
00045 r = rospy.Rate(rate)
00046
00047 self.joints = list()
00048
00049
00050 parameters = rospy.get_param_names()
00051
00052 for parameter in parameters:
00053
00054 if parameter.find(dynamixel_namespace) != -1 and parameter.find("joint_name") != -1:
00055 self.joints.append(rospy.get_param(parameter))
00056
00057 self.servos = list()
00058 self.controllers = list()
00059 self.joint_states = dict({})
00060
00061 for joint in self.joints:
00062
00063 servo = joint.split("_joint")[0]
00064 self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0)
00065
00066 self.controllers.append(dynamixel_namespace + servo + "_controller")
00067
00068 rospy.loginfo("Dynamixel Joint State Publisher " + joint)
00069
00070
00071 [rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
00072
00073
00074 self.joint_states_pub = rospy.Publisher('/joint_states', JointStatePR2)
00075
00076 rospy.loginfo("Starting Dynamixel Joint State Publisher at " + str(rate) + "Hz")
00077
00078 while not rospy.is_shutdown():
00079 self.publish_joint_states()
00080
00081 r.sleep()
00082 def reconfigure_callback(self, config, level):
00083 try:
00084 set_speed = rospy.ServiceProxy('tilt_controller/set_speed', SetSpeed)
00085 set_speed(config.tilt_speed)
00086 except rospy.ServiceException, e:
00087 print "Service call failed: %s"%e
00088 return config
00089 def controller_state_handler(self, msg):
00090 js = JointStateMessage(msg.name, msg.current_pos, msg.velocity, msg.load)
00091 self.joint_states[msg.name] = js
00092
00093 def publish_joint_states(self):
00094
00095 msg = JointStatePR2()
00096 msg.name = []
00097 msg.position = []
00098 msg.velocity = []
00099 msg.effort = []
00100
00101 for joint in self.joint_states.values():
00102 msg.name.append(joint.name)
00103 fudge_value = rospy.get_param('~fudge_factor/' + joint.name + '/value', 0.0)
00104 j_pos = joint.position - fudge_value
00105
00106 msg.position.append(j_pos)
00107 msg.velocity.append(joint.velocity)
00108 msg.effort.append(joint.effort)
00109
00110 msg.header.stamp = rospy.Time.now()
00111 self.joint_states_pub.publish(msg)
00112
00113 if __name__ == '__main__':
00114 try:
00115 s = JointStatePublisher()
00116 rospy.spin()
00117 except rospy.ROSInterruptException: pass
00118