def image_callback(msg): global start_t, g_dict, net, meta, cam_param, net_init, call_count, det_flag, has_rev if net_init: model_name = 'all_640' steps = '180000' base_dir = '/home/momenta/mc3/python/weight/weights-' + model_name net = dn.load_net( os.path.join(base_dir, "yolov3-tiny-" + model_name + ".cfg"), os.path.join( base_dir, "yolov3-tiny-" + model_name + "_" + steps + ".weights"), 0) meta = dn.load_meta( os.path.join(base_dir, "voc-" + model_name + ".data")) filename = "cam_top.yaml" f = open(filename) cam_param = yaml.load(f) net_init = False # commented by dongyan at 22:30 # call_count += 1 # if call_count%5 != 0: # return # stopped commented np_arr = np.fromstring(msg.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) g_isBallNear = isBallNear(image_np) print "g_isBallNear, ", g_isBallNear # if the ball is near, always push the ball if g_isBallNear: return t = time.time() - start_t if det_flag == True: print "det_flag == true, " g_dict = detect(net, meta, cam_param, image_np, isshow=True, issave=True, name=str(t)) has_rev = True else: g_dict = {"door": [], "football": [], "robot": [], "conf": []}
def image_callback(msg): global g_isBallNear, g_dict ''' global t # global blist global bdict,isClose global has_rev global det_flag ''' np_arr = np.fromstring(msg.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) g_isBallNear = isBallNear(image_np) # if the ball is near, always push the ball if g_isBallNear: return # use net to find ball filename = 'ori' + str(t) + '.jpg' cv2.imwrite(filename, image_np) # blist = detecttion(filename,flag=True,save=False) g_dict = detecttion(filename, save=False)
# motion_control.PointMove(0.9, -0.55, 0) # begin to move to a point. # motion_control.PointMove(0.5, 0, 0) motion_control.PointMove(1.35, -0.65, 0) isNearBaseLine = False carState = state_findBall else: if carState == state_findBall: bottomImg = findBallDoorTogether() # print "--------------------- <state> find ball ---------------------" if isBallNear(bottomImg): isNearBaseLine = False motion_control.PointMove(0.4, 0, 0) result = findElement(bottomImg) if len(result['football']) > 1: masterFindBall = True ballX, ballY = result['football'] if ballX > 0.2: # print "Move froward a little. NOT near baseline now." 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)
while not rospy.is_shutdown(): global masterFindBall, isSlaverAttacking if carState == state_begin: print "--------------------- <state> begin the battle ---------------------" motion_control.PointMove(0.8, -0.5, 0) # begin to move to a point. carState = state_findBall else: print "Find ball..." motion_control.rotateTo2(0) img = getPicture() if carState == state_findBall: print "--------------------- <state> find ball ---------------------" if isBallNear(img): print "mask is near ball but slaver is coming......" masterFindBall = True motion_control.PointMove(0.4, 0, 0) continue print "The ball is NOT near -- by HSV. Use yolo now." result = findElement(img, 'ball') if len(result['football']) > 1: masterFindBall = True ballX, ballY = result['football'] print "Find football at: ", ballX, ballY # anykey = raw_input("============= Press any key to Move to ball... =========== \n") if isSlaverAttacking == True: print "Slaving is attacking. [MASTER] STANDBY" continue
''' 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 else:
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
else: print "Angle error is large, = ", ballAngleError carState = state_findBall else: if carState == state_moveBack: rotateTo2(0) has_rev = False picTime = time.time() while (time.time()-picTime < 0.5): pass det_flag=True while has_rev == False: pass det_flag=False image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) if isBallNear(image_np): print "========== Near ball========" point_control2.PointMove(0.4, 0, 0) else: print "[state] state_moveBack" point_control2.PointMove(-0.2, 0, 0) moveback_times=moveback_times+1 carState = state_findBall else: print "[Error] Unexpected state"