platform_node2.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 # Copyright (c) 2009, Georgia Tech Research Corporation
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #     * Redistributions of source code must retain the above copyright
00008 #       notice, this list of conditions and the following disclaimer.
00009 #     * Redistributions in binary form must reproduce the above copyright
00010 #       notice, this list of conditions and the following disclaimer in the
00011 #       documentation and/or other materials provided with the distribution.
00012 #     * Neither the name of the Georgia Tech Research Corporation nor the
00013 #       names of its contributors may be used to endorse or promote products
00014 #       derived from this software without specific prior written permission.
00015 #
00016 # THIS SOFTWARE IS PROVIDED BY GEORGIA TECH RESEARCH CORPORATION ''AS IS'' AND
00017 # ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 # WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 # DISCLAIMED. IN NO EVENT SHALL GEORGIA TECH BE LIABLE FOR ANY DIRECT, INDIRECT,
00020 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00021 # LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
00022 # OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00023 # LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00024 # OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
00025 # ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 #
00027 #  \author Chih-Hung King (Healthcare Robotics Lab, Georgia Tech.)
00028 #   platform_node2.py controls the robotis on the hokuyo
00029 
00030 
00031 import roslib; roslib.load_manifest('tele_mobile')
00032 import math
00033 import time
00034 import sys
00035 
00036 import rospy
00037 import zenither.zenither as zenither
00038 import robotis.robotis_servo as rs
00039 import segway_omni.segway as segway
00040 
00041 from tele_mobile.msg import direction
00042 
00043 
00044 class Ctrl():
00045     def __init__(self,topic="tmobile"):
00046         self.sub = rospy.Subscriber(topic, direction,self.callback, None, 1)
00047         print 'start subscribing to topic', topic
00048         try:
00049             print 'initialize platform node'
00050             rospy.init_node("platform", anonymous=False)
00051         except rospy.ROSException, e:
00052             pass
00053 
00054 #               Robotis servo
00055         self.tilt = rs.robotis_servo('/dev/robot/servo0', 5, baudrate=57600)
00056         self.pan = rs.robotis_servo('/dev/robot/servo0', 25, baudrate=57600)
00057 #        self.tilt = rs.robotis_servo('/dev/robot/servo0', 26, baudrate=57600)
00058         self.angle1 = 0
00059         self.angle2 = 0
00060         self.reset = 0
00061 
00062 #               Zenither
00063         self.z = zenither.Zenither(robot='HRL2')
00064         self.z_dir = 0
00065         self.init_height = 1.   #initial zenither height = 1m
00066 
00067 #               Segway Omni
00068         self.mec = segway.Mecanum(init_ros_node=False)
00069         self.xvel = 0.
00070         self.yvel = 0.
00071         self.avel = 0.
00072         self.lock = 0.
00073 
00074 
00075 #       Callback funtion for rospy
00076     def callback(self, cmd):
00077         max_ang = 75
00078         min_ang = -75
00079         delta_ang = 2
00080         self.z_dir = cmd.zen
00081         self.xvel = cmd.xvel
00082         self.yvel = cmd.yvel
00083         self.avel = cmd.avel
00084         self.reset = cmd.reset
00085         self.zen_reset = cmd.zen_reset
00086         self.lock = cmd.lock
00087 
00088         if cmd.y == -1.0:
00089             if self.angle1 < math.radians(max_ang):
00090                 self.angle1 = self.angle1 + math.radians(delta_ang)
00091         elif cmd.y == 1.0:
00092             if self.angle1 > math.radians(min_ang):
00093                 self.angle1 = self.angle1 - math.radians(delta_ang)
00094         if cmd.x == -1.0:
00095             if self.angle2 < math.radians(max_ang):
00096                 self.angle2 = self.angle2 + math.radians(delta_ang)
00097         elif cmd.x == 1.0:
00098             if self.angle2 > math.radians(min_ang):
00099                 self.angle2 = self.angle2 - math.radians(delta_ang)    
00100         if cmd.reset == 1:      
00101             self.angle1 = 0
00102             self.angle2 = 0
00103 
00104 
00105     def set_servo(self):
00106         if self.reset == 1:
00107             print 'set servos to default position'
00108             self.pan.move_angle(0)
00109             self.tilt.move_angle(0)
00110 
00111         if (self.angle1 != 0 or self.angle2 != 0):
00112             print "tilt angle =", math.degrees(self.angle1),"pan angle=", math.degrees(self.angle2)
00113             self.pan.move_angle(self.angle2)
00114             self.tilt.move_angle(self.angle1)
00115 
00116 
00117     def set_zenither(self):
00118         self.move_zenither_flag = False
00119         if self.zen_reset == 1:
00120             curr_height = self.z.get_position_meters()    #get zenither height
00121             if curr_height == self.init_height:
00122                 print 'zenither height is at ',self.init_height,'m'                
00123             else:
00124                 self.z.torque_move_position(self.init_height,speed='slow')
00125                 torque = self.z.calib['zero_vel_torque']
00126                 print 'Reset zenither position to ',self.init_height,'m'
00127 
00128         if self.z_dir == 1.0:         
00129             curr_height = self.z.get_position_meters()    #get zenither height
00130             if curr_height <= (self.z.calib['max_height'] - 0.1):
00131                 self.z.nadir(self.z.calib['up_slow_torque'])
00132                 self.move_zenither_flag = True
00133                 print "zenither moving up to", curr_height
00134             else:
00135                 print 'max height threshold reached: ', curr_height
00136 
00137         if self.z_dir == -1.0:
00138             curr_height = self.z.get_position_meters()    #get zenither height
00139             if curr_height >= (self.z.calib['min_height'] + 0.40):
00140                 self.z.nadir(self.z.calib['down_snail_torque'])
00141                 self.move_zenither_flag = True
00142                 print "zenither moving down to", curr_height
00143             else:
00144                 print 'min height threshold reached: ', curr_height
00145 
00146         if not self.move_zenither_flag:
00147             self.z.estop()
00148 #            torque = self.z.calib['zero_vel_torque']
00149 #            print "zenither estop"
00150 
00151 
00152 #    def set_segway(self):
00153 #        if self.lock == 1.:
00154 #            print 'segway control is locked'
00155 #        if self.lock == 0.:
00156 #            self.mec.set_velocity(self.xvel, self.yvel, self.avel)
00157 
00158 
00159     def stop(self):
00160         print 'stop servos'
00161         self.tilt.disable_torque()
00162         time.sleep(0.1)
00163         self.pan.disable_torque()
00164         print 'estop zenither'
00165         self.z.estop()
00166         print 'stop segway'
00167         self.mec.set_velocity(0., 0., 0.)
00168         print 'stopping platform...'
00169         sys.exit()
00170   
00171 
00172 if __name__ == '__main__':
00173 
00174     platform = Ctrl()
00175 
00176     while not rospy.is_shutdown():
00177         platform.set_servo()
00178         platform.set_zenither()
00179         if platform.lock == 0.:
00180             print 'segway is locked'
00181         else:
00182             print 'segway is ready'
00183         platform.mec.set_velocity(platform.xvel,platform.yvel,platform.avel)
00184 
00185     platform.stop()


tele_mobile
Author(s): Chih-Hung King, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech
autogenerated on Wed Nov 27 2013 11:57:13