robotis_joint_controller.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
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 


pi_tracker
Author(s): Patrick Goebel
autogenerated on Tue Jan 7 2014 11:27:49