00001
00002
00003 """
00004 robotis_joint_controller.py - Version 1.0 2010-12-28
00005
00006 Joint Controller for AX-12 servos on a USB2Dynamixel device using
00007 the Robotis Controller Package from the Healthcare Robotics Lab at Georgia Tech
00008
00009 Created for the Pi Robot Project: http://www.pirobot.org
00010 Copyright (c) 2010 Patrick Goebel. All rights reserved.
00011
00012 This program is free software; you can redistribute it and/or modify
00013 it under the terms of the GNU General Public License as published by
00014 the Free Software Foundation; either version 2 of the License, or
00015 (at your option) any later version.
00016
00017 This program is distributed in the hope that it will be useful,
00018 but WITHOUT ANY WARRANTY; without even the implied warranty of
00019 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00020 GNU General Public License for more details at:
00021
00022 http://www.gnu.org/licenses/gpl.html
00023 """
00024
00025 import roslib; roslib.load_manifest('robotis')
00026 import rospy
00027 import servo_config as sc
00028 from robotis.lib_robotis import Robotis_Servo, USB2Dynamixel_Device
00029 from robotis.ros_robotis import ROS_Robotis_Server, ROS_Robotis_Client
00030 from sensor_msgs.msg import JointState
00031
00032 class joint_controller():
00033 def __init__(self):
00034 rospy.init_node('joint_controller')
00035
00036 rospy.on_shutdown(self.shutdown)
00037
00038 joint_state_pub = rospy.Publisher('/joint_states', JointState)
00039
00040 self.port = rospy.get_param('~port', '/dev/ttyUSB0')
00041 self.rate = rospy.get_param('~rate', 5)
00042 r = rospy.Rate(self.rate)
00043
00044 """ Read in the servo_config.py file. """
00045 self.servo_param = sc.servo_param
00046
00047 self.controllers = dict()
00048 self.joints = dict()
00049 self.ids = list()
00050
00051 """ Read servo ids and joint names from the servo config file. """
00052 for id in self.servo_param.keys():
00053 joint = self.servo_param[id]['name']
00054 self.joints[id] = joint
00055 self.ids.append(id)
00056
00057 """ Connect to the USB2Dynamixel controller """
00058 usb2dynamixel = USB2Dynamixel_Device(self.port)
00059
00060 """ Fire up the ROS joint server processes. """
00061 servos = [ Robotis_Servo( usb2dynamixel, i ) for i in self.ids ]
00062 ros_servers = [ ROS_Robotis_Server( s, str(j) ) for s, j in zip(servos, self.joints.values()) ]
00063
00064 """ Fire up the ROS services. """
00065 for joint in self.joints.values():
00066 self.controllers[joint] = ROS_Robotis_Client(joint)
00067
00068 """ Start the joint command subscriber """
00069 rospy.Subscriber('cmd_joints', JointState, self.cmd_joints_handler)
00070
00071 while not rospy.is_shutdown():
00072 """ Create a JointState object which we use to publish the current state of the joints. """
00073 joint_state = JointState()
00074 joint_state.name = list()
00075 joint_state.position = list()
00076 joint_state.velocity = list()
00077 joint_state.effort = list()
00078 for s in ros_servers:
00079 joint_state.name.append(s.name)
00080 joint_state.position.append(s.update_server())
00081 """ The robotis driver does not currently query the speed and torque of the servos,
00082 so just set them to 0. """
00083 joint_state.velocity.append(0)
00084 joint_state.effort.append(0)
00085 joint_state.header.stamp = rospy.Time.now()
00086 joint_state_pub.publish(joint_state)
00087 r.sleep()
00088
00089 def cmd_joints_handler(self, req):
00090 for joint in self.joints.values():
00091 try:
00092 self.controllers[joint].move_angle(req.position[req.name.index(joint)],
00093 max(0.01, req.velocity[req.name.index(joint)]), blocking=False)
00094 except:
00095 pass
00096
00097 def shutdown(self):
00098 rospy.loginfo('Shutting down joint command controller node...')
00099
00100
00101 if __name__ == '__main__':
00102 try:
00103 jc = joint_controller()
00104 except rospy.ROSInterruptException:
00105 pass
00106
00107
00108
