Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
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
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
00058 self.angle1 = 0
00059 self.angle2 = 0
00060 self.reset = 0
00061
00062
00063 self.z = zenither.Zenither(robot='HRL2')
00064 self.z_dir = 0
00065 self.init_height = 1.
00066
00067
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
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()
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()
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()
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
00149
00150
00151
00152
00153
00154
00155
00156
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()