#!/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