lookAroundCount = 0 # reset
elif carState == state_attack:
print "[state] Attacking!..."
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
result_dict = detect(net,meta,cam_param,image_np,isshow=False,issave=False,name="str(t)")
if len(result_dict["football"])>1:
x, y = result_dict["football"]
angle = math.atan2(y, x) * 180 / 3.1415926
rotateTo(angle, 0.2)
print "Attacking and rush to the ball."
point_control.PointMove(0.5, 0, 0)
defend_flag = 0 # change to attacking mode.
carState = state_findBall
while 1: # always attacking
if carState == state_findBall:
print "[state] findBall. Try to find ball"
# print "image_np:",image_np
has_rev = False
picTime = time.time()
while (time.time()-picTime < 0.5):
pass
det_flag=True
while has_rev == False:
carState = state_begin # begin with "findBall"
lookAroundCount = 0
while not rospy.is_shutdown():
global netInitFinished
global det_flag, has_rev, np_arr, rate
global g_dict, net, meta, cam_param, net_init, det_flag, has_rev, start_time
if netInitFinished == False: # wait for net to finish init
continue
if carState == state_begin:
print "[state] begin. Move to my door."
point_control.PointMove(0.3, 0, 0)
rotateTo(75, 0.6)
# anykey = raw_input("============= Press any key to continue =========== \n")
rotateTo(90, 0.3)
point_control.PointMove(0.5, 0, 0)
rotateTo(25, 0.6)
rotateTo(0, 0.3)
point_control.PointMove(-0.25, 0, 0)
cnt = 0
while 1:
print "Rotate ", cnt, " circles......"
rotateTo(0, 0.3)
rotateTo(90, 0.3)
rotateTo(179, 0.3)
rotateTo(-90, 0.3)
# state change part
if carState == carState_begin:
g_targetPointX, g_targetPointY = 0.1, 0.1
(currentPoint, currentRotation) = point_control.get_odom()
distance_error = math.sqrt(pow((currentPoint.x - g_targetPointX),2) + pow((currentPoint.y - g_targetPointY), 2))
if distance_error < 0.05:
carState = carState_searchBall
print "[State change]==> from carState_begin to carState_searchBall"
if carState == carState_searchBall:
# if find ball
carState = carState_chaseBall
print "[State change]==> from carState_searchBall to carState_chaseBall"
# if
if carState == carState_chaseBall:
if carState == carState_wait:
print "Waiting..."
# control part
if carState == carState_begin:
point_control.PointMove(g_targetPointX, g_targetPointY, 0)
if carState == carState_wait:
velocity_control.vel_control(0, 0)
print "[State]==> still carState_pushBall"
else:
if len(g_dict['football']) >= 1:
g_targetPointX, g_targetPointY = g_dict['football']
carState = carState_chaseBall
print "[State change]==> from carState_pushBall to carState_chaseBall"
else:
carState = carState_searchBall
print "[State change]==> from carState_pushBall to carState_searchBall"
else:
print "Waiting..."
# control part
if carState == carState_begin:
point_control.PointMove(g_targetPointX, g_targetPointY, 0)
elif carState == carState_searchBall:
velocity_control.vel_control(0, 2)
elif carState == carState_chaseBall:
while point_control.PointMove(g_targetPointX, g_targetPointY, 0) != 4:
pass
elif carState == carState_pushBall:
velocity_control.vel_control(5, 0)
# cv2.waitKey(1000)
else:
print "[Error], unexpected state..."
velocity_control.vel_control(0, 0)
# print(point_control.msg)
# (goal_x, goal_y, goal_z) = mymove.getkey()
# # mymove.PointMove(0.5,0.1,0)
# mymove.PointMove(goal_x, goal_y, goal_z)
# (position, rotation) = mymove.get_odom()
# print position, rotation
# rate.sleep()
# rospy.spin()
rospy.init_node('mc3_main', anonymous=True)
# mymove=point_control.GotoPoint()
velocity_control.init()
rate = rospy.Rate(10)
bridge = CvBridge()
rospy.Subscriber("/raspicam_node/image/compressed",
sensor_msgs.msg.CompressedImage, image_callback)
print 'hello'
while not rospy.is_shutdown():
if move_start == 0:
print(point_control.msg)
(goal_x, goal_y, goal_z) = point_control.getkey()
move_start = 1
else:
if point_control.PointMove(goal_x, goal_y, goal_z) == 4:
move_start = 0
# print velocity_control.msg
# (x,y)=velocity_control.getKey()
# velocity_control.vel_control(x,y)
rate.sleep()
print "================ a key is pressed ========================="
start_time = time.time()
while not rospy.is_shutdown():
global netInitFinished
global det_flag, has_rev, np_arr, rate
global g_dict, net, meta, cam_param, net_init, det_flag, has_rev, start_time, output_dir
if netInitFinished == False: # wait for net to finish init
continue
if carState == state_begin:
print "[state] begin. Move to middle point"
# start to move to the middle point.
# when begin, move to the middle of self-half square, then to find ball
point_control.PointMove(0.8, 0.6, 0)
'''
rpy=point_control.get_imu()
angle_error = 0 - rpy[2]
while abs(angle_error) > 5: # rotate to face front
rpy=point_control.get_imu()
angle_error = 0 - rpy[2]
if angle_error > 0:
velocity_control.vel_control(0, 0.4) # low speed to rotate to front
# rotateTo(angle_error)
else:
velocity_control.vel_control(0, 0.4)
# rotateTo(angle_error)
rate.sleep()
result_dict["football"] = [0.5, 0.2]
if len(result_dict["football"]) > 1:
carState = state_moveToPoint
targetPointX, targetPointY = result_dict['football']
print "Ball position: ", targetPointX, targetPointY
else:
print "Cannot find ball."
carState = state_moveBack
else:
if carState == state_moveToPoint:
print "[state] moveToPoint. "
print "POS:", [targetPointX, targetPointY]
point_control.PointMove(targetPointX - 0.3, targetPointY, 0)
print "Stopped at target point."
carState = state_rotateToFront
else:
if carState == state_rotateToFront:
print "[state] rotate to front. "
rpy = point_control.get_imu()
angle_error = 0 - rpy[2]
while abs(angle_error) > 5:
rpy = point_control.get_imu()
angle_error = 0 - rpy[2]
if angle_error > 0:
rate = rospy.Rate(10)
bridge = CvBridge()
point_control.init()
velocity_control.init()
point_control.reset_odom()
rospy.Subscriber("/raspicam_node/image/compressed",
sensor_msgs.msg.CompressedImage, image_callback)
velocity_control.vel_control(0.0, 0.0)
print 'hello'
goal_x, goal_y = [0, 0]
while not rospy.is_shutdown():
# print "ssr_stage=",ssr_stage
ssr_stage = 1
if ssr_stage == 0:
if point_control.PointMove(1.15, 0.4, 0) == 4:
print ssr_stage
print "ssr_stage 0 complete"
ssr_stage = 2
if ssr_stage == 1: #Stage1:find and catch ball
if move_start == 0: # detection
# print(point_control.msg)
# (goal_x, goal_y, goal_z) = point_control.getkey()
# move_start=1
det_flag = True
while not has_rev:
pass
# print "wait"
if bdict["football"] is None or len(
bdict["football"]) < 1: # if not detection return []
velocity_control.vel_control(0,
carState_pushBall = 'carState_pushBall'
carState_wait = 'carState_wait'
# carState = carState_begin
carState = carState_searchBall
# variables in control
g_targetPointX, g_targetPointY = 0, 0
g_targetSpeedLinear, g_targetSpeedAngular = 0, 0
# cv2.waitKey(10000)
start_time = time.time()
velocity_control.vel_control(0.0, 0.0)
point_control.PointMove(0, 0, 0)
anykey = raw_input("Press any key to start\n")
det_time = 0
while not rospy.is_shutdown():
global det_time
# print "----------------- State: ", carState, " ----------------------"
global g_targetPointX, g_targetPointY, g_targetSpeedLinear, g_targetSpeedAngular
global g_isBallNear, g_dict
global start_time
# print "<Attime>: ", round(time.time()-start_time), ", State: ", carState
# state change part
if carState == carState_begin:
