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