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 import roslib; roslib.load_manifest('tele_mobile')
00031 import math
00032 import time
00033 import sys
00034
00035 import rospy
00036 import zenither.zenither as zenither
00037 import robotis.robotis_servo as rs
00038 import segway_omni.segway as segway
00039
00040 from tele_mobile.msg import direction
00041
00042
00043 class Ctrl():
00044 def __init__(self,topic="tmobile"):
00045 self.sub = rospy.Subscriber(topic, direction,self.callback, None, 1)
00046 print 'start subscribing to topic', topic
00047 try:
00048 print 'initialize platform node'
00049 rospy.init_node("platform", anonymous=False)
00050 except rospy.ROSException, e:
00051 pass
00052
00053
00054
00055 self.pan = rs.robotis_servo('/dev/robot/servo0', 25, baudrate=57600)
00056 self.tilt = rs.robotis_servo('/dev/robot/servo0', 26, baudrate=57600)
00057 self.angle1 = 0
00058 self.angle2 = 0
00059 self.reset = 0
00060
00061
00062 self.z = zenither.Zenither(robot='HRL2')
00063 self.z_dir = 0
00064 self.init_height = 1.
00065 self.zen_reset = 0
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 def callback(self, cmd):
00076 max_ang = 75
00077 min_ang = -75
00078 delta_ang = 2
00079 self.z_dir = cmd.zen
00080 self.xvel = cmd.xvel
00081 self.yvel = cmd.yvel
00082 self.avel = cmd.avel
00083 self.reset = cmd.reset
00084 self.zen_reset = cmd.zen_reset
00085 self.lock = cmd.lock
00086
00087 if cmd.y == -1.0:
00088 if self.angle1 < math.radians(max_ang):
00089 self.angle1 = self.angle1 + math.radians(delta_ang)
00090 elif cmd.y == 1.0:
00091 if self.angle1 > math.radians(min_ang):
00092 self.angle1 = self.angle1 - math.radians(delta_ang)
00093 if cmd.x == -1.0:
00094 if self.angle2 < math.radians(max_ang):
00095 self.angle2 = self.angle2 + math.radians(delta_ang)
00096 elif cmd.x == 1.0:
00097 if self.angle2 > math.radians(min_ang):
00098 self.angle2 = self.angle2 - math.radians(delta_ang)
00099 if cmd.reset == 1:
00100 self.angle1 = 0
00101 self.angle2 = 0
00102
00103
00104 def set_servo(self):
00105 if self.reset == 1:
00106 print 'set servos to default position'
00107 self.pan.move_angle(0)
00108 self.tilt.move_angle(0)
00109
00110 if (self.angle1 != 0 or self.angle2 != 0):
00111 print "tilt angle =", math.degrees(self.angle1),"pan angle=", math.degrees(self.angle2)
00112 self.pan.move_angle(self.angle2)
00113 self.tilt.move_angle(self.angle1)
00114
00115
00116 def set_zenither(self):
00117 self.move_zenither_flag = False
00118 if self.zen_reset == 1:
00119 curr_height = self.z.get_position_meters()
00120 if curr_height == self.init_height:
00121 print 'zenither height is at ',self.init_height,'m'
00122 else:
00123 self.z.torque_move_position(self.init_height,speed='slow')
00124 torque = self.z.calib['zero_vel_torque']
00125 print 'Reset zenither position to ',self.init_height,'m'
00126
00127 if self.z_dir == 1.0:
00128 curr_height = self.z.get_position_meters()
00129 if curr_height <= (self.z.calib['max_height'] - 0.1):
00130 self.z.nadir(self.z.calib['up_slow_torque'])
00131 self.move_zenither_flag = True
00132 print "zenither moving up to", curr_height
00133 else:
00134 print 'max height threshold reached: ', curr_height
00135
00136 if self.z_dir == -1.0:
00137 curr_height = self.z.get_position_meters()
00138 if curr_height >= (self.z.calib['min_height'] + 0.40):
00139 self.z.nadir(self.z.calib['down_snail_torque'])
00140 self.move_zenither_flag = True
00141 print "zenither moving down to", curr_height
00142 else:
00143 print 'min height threshold reached: ', curr_height
00144
00145 if not self.move_zenither_flag:
00146 self.z.estop()
00147
00148
00149
00150
00151 def stop(self):
00152 print 'stop servos'
00153 self.tilt.disable_torque()
00154 time.sleep(0.1)
00155 self.pan.disable_torque()
00156 print 'estop zenither'
00157 self.z.estop()
00158 print 'stop segway'
00159 self.mec.set_velocity(0., 0., 0.)
00160 print 'stopping platform...'
00161 sys.exit()
00162
00163
00164 if __name__ == '__main__':
00165
00166 platform = Ctrl()
00167
00168 while not rospy.is_shutdown():
00169 platform.set_servo()
00170 platform.set_zenither()
00171 if platform.lock == 0.:
00172 print 'segway is locked'
00173 else:
00174 print 'segway is ready'
00175 platform.mec.set_velocity(platform.xvel,platform.yvel,platform.avel)
00176
00177 platform.stop()