isNearBaseLine = False
print "Find football at: ", ballX, ballY
if abs(ballY) < 0.1: # the ball is in the front
print "Robot is facing the ball. Move forward."
motion_control.PointMove(ballX + 0.4, 0, 0)
else:
# print "Robot is NOT facing the ball, try to go to the behind."
if ballY > 0:
motion_control.PointMove(ballX - 0.2, ballY + 0.04, 0)
else:
motion_control.PointMove(ballX - 0.2, ballY - 0.04, 0)
else:
# print "No ball found by yolo, ",
if isCarNear(bottomImg):
print "Car! Car! Car! Car! Car! Car!"
continue
else:
# print "a car is NOT near. Change to moveback"
carState = state_changePosition
if carState == state_changePosition:
# print "--------------------- <state> change position ---------------------"
result = findElement_top(bottomImg)
if len(result['door']) == 0:
print "<DOOR> Cannot find door. Move back safely..."
# motion_control.rotateTo2(0)
motion_control.PointMove(-0.3, 0, 0)
else:
doorX, doorY = 0, 0
# anykey = raw_input("============= Press any key to Move to ball... =========== \n")
if isSlaverAttacking == True:
print "Slaving is attacking. [MASTER] STANDBY"
continue
else:
if abs(ballY) < 0.1: # the ball is in the front
print "Robot is facing the ball. Move forward."
motion_control.PointMove(ballX + 0.4, 0, 0)
else:
print "Robot is NOT facing the ball, try to go to the behind."
motion_control.PointMove(ballX - 0.2, ballY, 0)
else:
print "No ball found by yolo, ",
masterFindBall = False
if isCarNear(img):
print "a car is NEAR. Do not move back"
continue
else:
print "a car is NOT near. Change to moveback"
carState = state_changePosition
if carState == state_changePosition:
print "--------------------- <state> change position ---------------------"
# motion_control.rotateTo2(0)
result = findElement(img, 'door') # find door info
if len(result['door']) == 0:
print "<DOOR> Cannot find door. Move back safely..."
motion_control.rotateTo2(0)
motion_control.PointMove(-0.3, 0, 0)
continue
# carState remains to be state_findBall
else:
print "Cannot find football by Yolo. Wil change position..."
slaverFindBall = False
slaver_publish()
carState = state_changePosition
elif carState == state_changePosition:
print "--------------------- <state> change position ---------------------"
# anykey = raw_input("============= Press any key to continue =========== \n")
motion_control.rotateTo2(0)
if isCarNear(getPicture()):
print "Car is near. Do not move at all."
# anykey = raw_input("============= Press any key to continue =========== \n")
carState = state_findBall
continue
print "Car is NOT near. Try to find door and move back."
img = getPicture_top() # get top camera's picture.
result = findElement_top(img)
if len(result['door']) == 0:
print "<DOOR> Cannot find door. Move back safely..."
motion_control.rotateTo2(0)
motion_control.PointMove(-0.2, 0, 0)
else:
# doorInfo = result['door']
doorX, doorY = 0, 0
'''
import cv2
# import by dongyan
from checkBallNear_release import isBallNear
from checkCarNear_release import isCarNear
i = 0
FILE_PATH = "rubishbin/"
while True: # 'q' or Esc
img = cv2.imread(FILE_PATH + str(i) + ".jpg")
car_near = isCarNear(img)
ball_near = isBallNear(img)
print "---------------"
print "No.", i, ", Car:", car_near, ", Ball: ", ball_near
cv2.imshow("Source", img)
keyValue = cv2.waitKey(0)
if keyValue == 113 or keyValue == 27: #q or "Esc"
break
elif keyValue == 97:
i = i - 1
if i < 0:
i = 0
elif keyValue == 98:
i = 0
sensor_msgs.msg.CompressedImage, image_callback)
rospy.Subscriber("/raspicam_node_top/image/compressed",
sensor_msgs.msg.CompressedImage, image_callback_top)
goal_x, goal_y = [0, 0]
# ---------------------------------init--------------------------------------------
state_begin = 'state_begin'
state_findBall = 'state_findBall'
state_changePosition = 'state_changePosition'
state_wait = 'state_wait'
state_unexpected = 'state_unexpected'
carState = state_begin # begin with "findBall"
targetPointX, targetPointY = 0, 0
print "================ waiting for odom to reset ========================="
anykey = raw_input("============= Press any key to start =========== \n")
print "================ a key is pressed ========================="
start_time = time.time()
isMyHalf = True
moveBackCount = 0
while not rospy.is_shutdown():
anykey = raw_input(
"============= Press any key to continue =========== \n")
img = getPicture()
ballNear, carNear = isBallNear(img), isCarNear(img)
print "ball, car: ", ballNear, ", ", carNear
