#!/usr/bin/env python #coding=utf-8 import rospy import numpy as np from Python_API import Sendmessage import time send = Sendmessage() class view_message: def __init__(self): #影像位置 self.basket_X = 0 #框 self.basket_XMin = 0 self.basket_XMax = 0 self.basket_Y = 0 self.basket_YMin = 0 self.basket_YMax = 0 self.ball_X = 0 #球 self.ball_XMin = 0 self.ball_XMax = 0 self.ball_Y = 0 self.ball_YMin = 0 self.ball_YMax = 0 self.white_X = 0
red_middle = float(target_xmax + target_xmin) / 2 print('middle=', red_middle) if red_middle > 172: send.sendContinuousValue(0, -500, 0, 0, 0) print('move right') if red_middle < 157: send.sendContinuousValue(0, 500, 0, 0, 0) print('move left') if red_middle > 157 and red_middle < 172: arrive = True return arrive imgdata = [[None for high in range(240)] for width in range(320)] if __name__ == '__main__': send = Sendmessage() try: #find_white_line=True #lift_bar=True #pick_bar = True time.sleep(0.35) send.sendSensorReset() send.sendHeadMotor(2, 1450, 50) r = rospy.Rate(5) #5hz while not rospy.is_shutdown(): if send.is_start == True: print(Body_Auto) if lift_bar == False: if lift_line == False: #if find_white_line==False:
if -11 <= s[0]-Find_Target()[0] <= 11 and -11 <= s[1]-Find_Target()[1] <= 11 and k == 0 : print("開始計時") start =time.time() # time.sleep(2) k = 1 if -11 <= s[0]-Find_Target()[0] <= 11 and -11 <= s[1]-Find_Target()[1] <= 11 and k == 1 : end = time.time() return start,end if __name__ == '__main__': try: send = Sendmessage() m = 0 mi = 0 msum = 0 startM = 0 endM = 0 g = 0 h = 0 i = 0 hh = 0 hhll = 0 r = rospy.Rate(5) k = 0 hl = 0 lll =0 hll = 0
#!/usr/bin/env python #coding=utf-8 import rospy import numpy as np from Python_API import Sendmessage from find_ladder_API import Find_ladder from SR_API import Send_distance imgdata = [[None for high in range(240)]for width in range (320)] if __name__ == '__main__': try: send = Sendmessage() #建立名稱,順便歸零,就是底線底線init ladder = Find_ladder() distance = Send_distance() send.drawImageFunction(1,0,0,320,ladder.eyeline_y,ladder.eyeline_y,255,255,255) #眼睛的橫線 send.drawImageFunction(2,0,ladder.eyeline_x,ladder.eyeline_x,0,240,255,255,255) #垂直線 # send.drawImageFunction(3,0,ladder.eye_r,ladder.eye_r,0,240,255,255,255) #右線 send.sendHeadMotor(2,ladder.head_highest,100)#頭回到原位 while not rospy.is_shutdown(): if send.is_start == True: print('start') # print('start') # ladder.find_all() elif send.is_start == False:
#coding=utf-8 import rospy import numpy as np from Python_API import Sendmessage from Ladder_API import Send_Climb import time #imgdata = [[None for high in range(240)]for width in range (320)] # lrdistance = 0 # lldistance = 0 # rldistance = 0 # rrdistance = 0 if __name__ == '__main__': try: send = Sendmessage() #建立名稱,順便歸零,就是底線底線init climb = Send_Climb() #建立名稱,順便歸零 while not rospy.is_shutdown(): #判斷Humanoid Interface的按鈕 #if send.Web == True: if send.is_start == True: send.drawImageFunction(1, 0, 0, 320, 215, 215, 255, 0, 0) #膝蓋的橫線 send.drawImageFunction(2, 0, 98, 98, 0, 240, 255, 0, 0) #ll的線 send.drawImageFunction(3, 0, 150, 150, 0, 240, 255, 0, 0) #lr的線 send.drawImageFunction(4, 0, 188, 188, 0, 240, 255, 0, 0) #rl的線 send.drawImageFunction(5, 0, 240, 240, 0, 240, 255, 0, 0) #rr的線
import rospy import numpy as np from Python_API import Sendmessage from SR_API import Send_distance from SR_API_2 import Ladder_send_distance from SR_API_ladder import Climb_ladder import time imgdata = [[None for high in range(240)] for width in range(320)] if __name__ == '__main__': try: send = Sendmessage() #建立名稱,順便歸零,就是底線底線init distance = Send_distance() #建立名稱,順便歸零 Ladder = Ladder_send_distance() Climb = Climb_ladder() send.is_start = False while not rospy.is_shutdown(): if send.is_start == True: #=====================鏡頭上畫線(name,line or square,xmin,xmax,ymin,ymax,r,g,b)===================== send.drawImageFunction(1, 0, 0, 320, 230, 230, 255, 0, 0) #膝蓋的橫線 send.drawImageFunction( 2, 0, 150, 150, 0, 240, 255, 0, 0) #lr的線 #lr代表〝左腳〞的〝右邊〞那條線 send.drawImageFunction(3, 0, 115, 115, 0, 240, 255, 0,
#!/usr/bin/env python #coding=utf-8 import rospy import numpy as np from Python_API import Sendmessage import time send = Sendmessage() def yaw_forward(x): global yaw_hold global theta if (x) > yaw_hold + 3: print("turn right") print(x) theta = -1 elif (x) < yaw_hold - 3: print("turn left") print(x) theta = 2 else: theta = 2 return theta def yaw_backward(bx): global yaw_hold global theta if (bx) > yaw_hold + 3:
# 5/15 #!/usr/bin/env python #coding=utf-8 import rospy import numpy as np from rospy import Publisher from tku_msgs.msg import Interface,HeadPackage,SandHandSpeed,DrawImage,SingleMotorData,\ SensorSet,ObjectList,LabelModelObjectList,RobotPos,SetGoalPoint,SoccerDataList,SensorPackage from std_msgs.msg import Int16, Bool from sensor_msgs.msg import Image from cv_bridge import CvBridge, CvBridgeError import cv2 import time from Python_API import Sendmessage send = Sendmessage() #要放在class外面,不然不能宣告 class Send_distance(): def __init__(self): #初始化 #腳掌標準線x值 self.knee = 215 self.f_ll = 98 self.f_lr = self.f_ll + 52 self.f_rl = 170 self.f_rr = self.f_rl + 52 self.head_Horizontal = 2040 #self.head_Vertical = 1425 self.head_Vertical = 1392 #距離矩陣初始化 self.up_distance = [999, 999, 999, 999] #要上的層
finish_turn_left_flag=0#完成左轉 finish_turn_right_flag=0#完成右轉 turn_now_flag=0 #---------------------------------------------------------------------- #第二階段旗標 second_part_flag=0 #成功判斷銀幕內有箭頭 go_to_second_part_flag=0#線段只有在銀幕下方 next_stage_flag=0 #修正是否正對箭頭 #---------------------------------------------------------------------- #步態初始化 origin_theta=1 origin_Y=0 if __name__ == '__main__': try: send = Sendmessage() r=rospy.Rate(5) while not rospy.is_shutdown(): send.sendHeadMotor(2,1600,50) send.sendHeadMotor(1,2048,50) if send.is_start == True: if start == True: initial() send.sendSensorReset() time.sleep(0.5) send.sendBodyAuto(0,0,0,0,1,0) start=False else: if second_part_flag==1 and go_to_second_part_flag==1:#判斷是否有箭頭與是否要進入第二階段 straight_temp, right_temp, left_temp, arrow_center_y, arrow_center_x=camera(straight_temp, right_temp, left_temp) second_part_flag, turn_right_flag, turn_left_flag=arrow_flag(straight_temp, right_temp, left_temp, second_part_flag, turn_right_flag, turn_left_flag)
#!/usr/bin/env python #coding=utf-8 import rospy import numpy as np from Python_API import Sendmessage from SR_API_test import Send_distance from Ladder_API import Send_Climb import time imgdata = [[None for high in range(240)] for width in range(320)] if __name__ == '__main__': try: send = Sendmessage() #建立名稱,順便歸零,就是底線底線init distance = Send_distance() #建立名稱,順便歸零 climb = Send_Climb() r = rospy.Rate(5) send.sendBodySector(29) while not rospy.is_shutdown(): #判斷Humanoid Interface的按鈕 # if send.Web == True: if send.is_start == True: if send.DIOValue == 31 or send.DIOValue == 15 or send.DIOValue == 23 or send.DIOValue == 7: distance.print_state() send.drawImageFunction(1, 0, 0, 320, distance.knee, distance.knee, 255, 0, 0) #膝蓋的橫線 send.drawImageFunction(2, 0, distance.f_lr, distance.f_lr, 0, 240, 255, 0, 0) #lr的線 # send.drawImageFunction(3,0,132,132,0,240,255,0,0)#lm的線 send.drawImageFunction(4, 0, distance.f_ll, distance.f_ll, 0, 240, 255, 0, 0) #ll的線 send.drawImageFunction(5, 0, distance.f_rl, distance.f_rl, 0, 240, 255, 0, 0) #rl的線
#!/usr/bin/env python #coding=utf-8 from cmath import sqrt import time from traceback import print_tb import numpy as np import rospy from Python_API import Sendmessage send=Sendmessage() class target_location(): def __init__(self): self.ball_x = 0 self.ball_y = 0 self.basket_x = 0 self.basket_y = 0 self.ball_size = 0 self.basket_size = 0 self.color_mask_subject_red = 0 self.color_mask_subject_orange = 0 self.ball_x_min = 0 self.ball_y_min = 0 self.ball_x_max = 0 self.ball_y_max = 0 self.basket_x_min = 0 self.basket_y_min = 0 self.basket_x_max = 0 self.basket_y_max = 0