00001 import roslib 00002 roslib.load_manifest('trigger_msgs') 00003 import sys 00004 import rospy 00005 import dynamic_reconfigure.client 00006 import time 00007 import trigger_msgs.msg 00008 import geometry_msgs.msg as gm 00009 import time 00010 import sys 00011 00012 dist = float(sys.argv[1]) 00013 print dist 00014 rospy.init_node("move_back", anonymous=True) 00015 move_base = rospy.Publisher('simple_move_base', gm.Pose2D) 00016 r = rospy.Rate(10) 00017 p2d = gm.Pose2D() 00018 p2d.x = dist 00019 p2d.y = .15 00020 print 'move_back -.6' 00021 r.sleep() 00022 move_base.publish(p2d) 00023 r.sleep() 00024 r.sleep() 00025 r.sleep() 00026 r.sleep() 00027 r.sleep() 00028 00029 #import threading as tr 00030 00031 #class ProjectorWakeUp(threading.Thread): 00032 # 00033 # def __init__(self): 00034 # threading.Thread.__init__(self) 00035 # self.projector = dynamic_reconfigure.client.Client('camera_synchronizer_node') 00036 # 00037 # def run(self): 00038 # self.projector.update_configuration(projector_on) 00039 # time.sleep(.2) 00040 # self.projector.update_configuration(projector_off) 00041 # time.sleep(.05) 00042 # self.projector.update_configuration(projector_on) 00043 # time.sleep(.05) 00044 # self.projector.update_configuration(projector_off) 00045 # time.sleep(2) 00046 # self.projector.update_configuration(projector_on) 00047 00048 #head_up = rospy.Publisher("head_up", trigger_msgs.msg.Trigger, latch = True) 00049 #head_down = rospy.Publisher("head_down", trigger_msgs.msg.Trigger, latch = True) 00050 #head_down_up = rospy.Publisher("head_down_up", trigger_msgs.msg.Trigger, latch = True) 00051 #arm_on = rospy.Publisher("light_on", trigger_msgs.msg.Trigger, latch = True) 00052 #arm_off = rospy.Publisher("light_off", trigger_msgs.msg.Trigger, latch = True) 00053 # 00054 #left_initial_pose = rospy.Publisher("left_start", trigger_msgs.msg.Trigger, latch = True) 00055 #left_initial_pose0 = rospy.Publisher("left_start2", trigger_msgs.msg.Trigger, latch = True) 00056 #right_initial_pose0 = rospy.Publisher("right_initial_pose0", trigger_msgs.msg.Trigger, latch = True) 00057 ##right_initial_pose00 = rospy.Publisher("right_initial_pose00", trigger_msgs.msg.Trigger, latch = True) 00058 #froo_froo = rospy.Publisher("froo_froo", trigger_msgs.msg.Trigger, latch = True) 00059 #head_look_around = rospy.Publisher("head_look_around2", trigger_msgs.msg.Trigger, latch = True) 00060 # 00061 #both_arms_forward2 = rospy.Publisher("both_arms_forward2", trigger_msgs.msg.Trigger, latch = True) 00062 #both_arms_fold2 = rospy.Publisher("both_arms_fold2", trigger_msgs.msg.Trigger, latch = True) 00063 #both_arms_fold_end_pose = rospy.Publisher("both_arms_fold_end_pose", trigger_msgs.msg.Trigger, latch = True) 00064 #head_turn = rospy.Publisher("head_turn", trigger_msgs.msg.Trigger, latch = True) 00065 # 00066 #arm_spin = rospy.Publisher("arm_spin", trigger_msgs.msg.Trigger, latch = True) 00067 #raise_the_roof = rospy.Publisher("raise_the_roof", trigger_msgs.msg.Trigger, latch = True) 00068 #head_up_full = rospy.Publisher("head_up_full", trigger_msgs.msg.Trigger, latch = True) 00069 #head_down_full = rospy.Publisher("head_down_full", trigger_msgs.msg.Trigger, latch = True) 00070 00071 #hand_up = rospy.Publisher("hand_up", trigger_msgs.msg.Trigger, latch = True) 00072 #hand_down = rospy.Publisher("hand_down", trigger_msgs.msg.Trigger, latch = True) 00073 #left_initial_pose00 = rospy.Publisher("left_initial_pose00", trigger_msgs.msg.Trigger, latch = True) 00074 # right_initial_pose00 = rospy.Publisher("right_initial_pose00", trigger_msgs.msg.Trigger, latch = True) 00075 #right_initial_pose = rospy.Publisher("right_initial_pose", trigger_msgs.msg.Trigger, latch = True) 00076 00077 #projector_on = {'camera_reset': False, 00078 # 'forearm_l_rate': 30.0, 00079 # 'forearm_l_trig_mode': 1, 00080 # 'forearm_r_rate': 30.0, 00081 # 'forearm_r_trig_mode': 1, 00082 # 'narrow_stereo_trig_mode': 2, 00083 # 'projector_mode': 3, 00084 # 'projector_pulse_length': 0.002, 00085 # 'projector_pulse_shift': 0.0, 00086 # 'projector_rate': 58.823529411764703, 00087 # 'projector_tweak': 0.0, 00088 # 'prosilica_projector_inhibit': False, 00089 # 'stereo_rate': 29.411764705882351, 00090 # 'wide_stereo_trig_mode': 2} 00091 # 00092 # 00093 #projector_off = {'camera_reset': False, 00094 # 'forearm_l_rate': 30.0, 00095 # 'forearm_l_trig_mode': 1, 00096 # 'forearm_r_rate': 30.0, 00097 # 'forearm_r_trig_mode': 1, 00098 # 'narrow_stereo_trig_mode': 2, 00099 # 'projector_mode': 1, 00100 # 'projector_pulse_length': 0.002, 00101 # 'projector_pulse_shift': 0.0, 00102 # 'projector_rate': 58.823529411764703, 00103 # 'projector_tweak': 0.0, 00104 # 'prosilica_projector_inhibit': False, 00105 # 'stereo_rate': 29.411764705882351, 00106 # 'wide_stereo_trig_mode': 2} 00107 #projector = dynamic_reconfigure.client.Client('camera_synchronizer_node') 00108 00109 00110 #r = rospy.Rate(120/60.) 00111 #time.sleep(.2) 00112 #r.sleep() 00113 #projector_animation = ProjectorWakeUp() 00114 #i = -23 00115 #while not rospy.is_shutdown(): 00116 # 00117 # print '------------', i, '-------------' 00118 # if i == -23: 00119 # print 'left_initial_pose0' 00120 # left_initial_pose0.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 1.5)), rospy.get_param("~event", "")) 00121 # 00122 # if i == -12: 00123 # print 'left_initial_pose' 00124 # left_initial_pose.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 2.0)), rospy.get_param("~event", "")) 00125 # 00126 # if i == -6: 00127 # print 'arm_on' 00128 # arm_on.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.3)), rospy.get_param("~event", "")) 00129 # 00130 # if i == -5: 00131 # print 'head_look_around' 00132 # head_look_around.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.2)), rospy.get_param("~event", "")) 00133 # 00134 # if i >= 9 and i <= 49: 00135 # arm_i = (i - 9) 00136 # if arm_i % 8 == 0: 00137 # print 'lights off' 00138 # arm_off.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", .8)), rospy.get_param("~event", "")) 00139 # 00140 # if arm_i % 8 == 4: 00141 # print 'lights on' 00142 # arm_on.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", .8)), rospy.get_param("~event", "")) 00143 # 00144 # if i >= 15 and i <= 34: 00145 # head_i = i - 15 00146 # if head_i % 4 == 0: 00147 # #Down 00148 # print 'down' 00149 # head_down_up.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.1)), rospy.get_param("~event", "")) 00150 # #head_up.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.0)), rospy.get_param("~event", "")) 00151 # 00152 # if head_i % 4 == 2: 00153 # #Up 00154 # print 'up' 00155 # #head_down.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.8)), rospy.get_param("~event", "")) 00156 # #head_up.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.0)), rospy.get_param("~event", "")) 00157 # #hand_up.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.0)), rospy.get_param("~event", "")) 00158 # 00159 # if i >= 40 and i <= 43: 00160 # head_i = i - 41 00161 # if head_i % 4 == 0: 00162 # head_down_up.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.1)), rospy.get_param("~event", "")) 00163 # print 'down' 00164 # 00165 # if head_i % 4 == 2: 00166 # print 'up' 00167 # #head_up.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.8)), rospy.get_param("~event", "")) 00168 # 00169 # ################################################################################ 00170 # ### FREESTYLE 00171 # ################################################################################ 00172 # if i == 23: 00173 # print 'right_initial_pose0' 00174 # right_initial_pose0.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.3)), rospy.get_param("~event", "")) 00175 # 00176 # #if i == 24: 00177 # # print 'right_initial_pose0' 00178 # # right_initial_pose00.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 3.5)), rospy.get_param("~event", "")) 00179 # 00180 # if i == 26: 00181 # #if i == 29: 00182 # print 'arm_spin' 00183 # arm_spin.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 3.0)), rospy.get_param("~event", "")) 00184 # 00185 # #if i >= 29 and i < 37: 00186 # # if ((i-29) % 9) == 0: 00187 # # print 'Free style!' 00188 # # froo_froo.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 2.0)), rospy.get_param("~event", "")) 00189 # 00190 # if i == 42: 00191 # #if i == 45: 00192 # #ANOTHER FREESTYLE 00193 # print 'raise_the_roof' 00194 # raise_the_roof.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 3.0)), rospy.get_param("~event", "")) 00195 # 00196 # ############################################################################### 00197 # ## Dancy 00198 # ############################################################################### 00199 # if i == 53: 00200 # print 'head_down' 00201 # print 'both_arms_forward2' 00202 # both_arms_forward2.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 2)), rospy.get_param("~event", "")) 00203 # 00204 # if i == 56: 00205 # p2d = gm.Pose2D() 00206 # p2d.x = -.4 00207 # p2d.y = .15 00208 # move_base.publish(p2d) 00209 # head_down_full.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.5)), rospy.get_param("~event", "")) 00210 # 00211 # if i == 61: 00212 # print 'head_up' 00213 # print 'both_arms_fold2' 00214 # head_up_full.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.4)), rospy.get_param("~event", "")) 00215 # both_arms_fold2.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.8)), rospy.get_param("~event", "")) 00216 # 00217 # if i == 65: 00218 # p2d = gm.Pose2D() 00219 # p2d.y = 100. 00220 # p2d.theta = -390 00221 # move_base.publish(p2d) 00222 # 00223 # if i == 77: 00224 # print 'both_arms_fold_end_pose' 00225 # print 'head_turn' 00226 # head_turn.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 7.0)), rospy.get_param("~event", "")) 00227 # both_arms_fold_end_pose.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 0.8)), rospy.get_param("~event", "")) 00228 # 00229 # i = i+1 00230 # r.sleep() 00231 # if i == 80: 00232 # break 00233 00234 #projector.update_configuration(projector_off) 00235 00236 00237 00238 00239 00240 00241 00242 00243 00244 00245 00246 00247 00248 00249 00250 00251 00252 00253 #if i == -12: 00254 # left_initial_pose00.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 5)), rospy.get_param("~event", "")) 00255 # right_initial_pose00.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 5)), rospy.get_param("~event", "")) 00256 00257 #if i == 43: 00258 # right_initial_pose.publish(rospy.get_rostime() + rospy.Duration(rospy.get_param("~delay", 3.)), rospy.get_param("~event", ""))