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"
