dynamixel_joint_state_publisher.py
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 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         # Get all parameter names
00050         parameters = rospy.get_param_names()
00051         
00052         for parameter in parameters:
00053             # Look for the parameters that name the joints.
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             # Remove "_joint" from the end of the joint name to get the controller names.
00063             servo = joint.split("_joint")[0]
00064             self.joint_states[joint] = JointStateMessage(joint, 0.0, 0.0, 0.0)
00065             #  ARD
00066             self.controllers.append(dynamixel_namespace + servo + "_controller")
00067             # self.controllers.append(joint)
00068             rospy.loginfo("Dynamixel Joint State Publisher " + joint)
00069                            
00070         # Start controller state subscribers
00071         [rospy.Subscriber(c + '/state', JointStateDynamixel, self.controller_state_handler) for c in self.controllers]
00072      
00073         # Start publisher
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             # With 23 servos, we go as fast as possible
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         # Construct message & publish joint states
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             # rospy.loginfo("fudge " + str(joint.name) + ': ' + str(j_pos) + ' = ' + str(joint.position) + ' - ' + str(fudge_value))
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 


jsk_tilt_laser
Author(s): YoheiKakiuchi
autogenerated on Sun Jan 25 2015 12:36:53