def rotateTo2(angle): # rotate to a certain angle, with default speed 0.4
rate = rospy.Rate(100)
# global rate
rpy=point_control2.get_imu()
angle_error = angle - rpy[2]
if(angle_error > 180):
angle_error -= 360
elif(angle_error < -180):
# print "angle_error,", angle_error
angle_error += 360
# print "angle_error,", angle_error
# print "<rotateTo> Input angle:", angle, "current angle: ",rpy[2], "angle error:", angle_error
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
angle_error_old = 0
while abs(angle_error) > 5:
rpy=point_control2.get_imu()
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
angle_error_old = angle_error # pd
angle_error = angle - rpy[2]
if(angle_error > 180):
angle_error -= 360
elif(angle_error < -180):
angle_error += 360
# speed = angle_error * 0.04 + (angle_error - angle_error_old) * 0.02
# speed = max(angle_error*0.02, 0.4) #lowerspped, 0.4
speed = angle_error * 0.03
if speed > 0:
speed = min(speed, 2.0) # upperspeed, 2
else:
speed = max(speed, -2.0)
velocity_control.vel_control(0, speed) # low speed to rotate to front
# print "<rotateTo> Stopped: ",rpy[2], "angle error:", angle_error
# print "angle_error", angle_error
rate.sleep()
# print "<rotateTo> Rotate finished at angle: ", rpy[2]
velocity_control.vel_control(0, 0)
def rotateTo(angle,
speed=0.4): # rotate to a certain angle, with default speed 0.4
global rate
rpy = point_control.get_imu()
angle_error = angle - rpy[2]
print "<rotateTo> Input angle:", angle, "current angle: ", rpy[
2], "angle error:", angle_error
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
while abs(angle_error) > 5:
rpy = point_control.get_imu()
# print "angle_error: ", angle_error,"angle_now: ", rpy[2]
angle_error = angle - rpy[2]
if angle_error > 0:
velocity_control.vel_control(0,
speed) # low speed to rotate to front
else:
velocity_control.vel_control(0, -speed)
# print "angle_error", angle_error
rate.sleep()
print "<rotateTo> Rotate finished at angle: ", rpy[2]
velocity_control.vel_control(0, 0)
def back(distance):
(position_start, rotation) = point_control.get_odom()
(position, rotation) = point_control.get_odom()
while (position_start.x - position.x) < distance:
velocity_control.vel_control(-0.5, 0.0)
(position, rotation) = point_control.get_odom()
(position_start, rotation) = point_control.get_odom()
(position, rotation) = point_control.get_odom()
while (position_start.x - position.x) < distance:
velocity_control.vel_control(-0.5, 0.0)
(position, rotation) = point_control.get_odom()
rospy.init_node('mc3_main', anonymous=True)
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]
base_x = 0
base_y = 0
base_z = 0
state3_flag = True
move2flag = False
while not rospy.is_shutdown():
# print "ssr_stage=",ssr_stage
ssr_stage = 0
if ssr_stage == 0:
distance = raw_input("distance\n")
back(distance)
# if point_control.PointMove(1.15, 0.4, 0)==4:
pts_c = np.concatenate((pts_a_xy,-dz_a),axis = 1)
# Calculate the natural error
R_af, t_af = six_DOF_transformation(pts_c,pts_a) # transformation of g w.r.t g_a
x = R2rpy(R_af)
delta_R = np.array([[np.cos(x[0]), -np.sin(x[0]),0],[np.sin(x[0]), np.cos(x[0]),0],[0,0,1]])
R_af = np.matmul(np.transpose(delta_R),R_af)
R_ap = np.matmul(R_af,R_fp)
t_ap = np.matmul(R_af,t_fp)+t_af
e_R, e_t = cal_error(R_ap_target,t_ap_target,R_ap,t_ap)
Kw = np.array([[1,0,0],[0,1,0],[0,0,1]])
Kv = np.array([[9,0,0],[0,12,0],[0,0,8]])
wb,vb = vel_control(Kw, Kv, e_R, e_t,wb,vb)
ws = np.matmul(p_R,wb)
d_p_R = exp_map_SO3(ws*0.1)
a,b,c = R2rpy(d_p_R)
# control r,p y
a = 1*a
b = 10*b
c = 10*c
d_p_R = rpy2R(a,b,c)
dws = log_map_SO3(d_p_R)
prev_a = a
prev_b = b
prev_c = c
vs = np.matmul(p_R,vb) + np.matmul(np.matmul(skew(p_t),p_R),wb)
filename = 'ori' + str(t) + '.jpg'
cv2.imwrite(filename, image_np)
# blist = detecttion(filename,flag=True,save=False)
g_dict = detecttion(filename, save=False)
#--------------------------------- init--------------------------------------------
rospy.init_node('mc3_main', anonymous=True)
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]
# ---------------------------------init--------------------------------------------
# ---------------------- new by Dong yan-----------------------
# car state machine
carState_begin = 'carState_begin'
carState_searchBall = 'carState_searchBall'
carState_chaseBall = 'carState_chaseBall'
carState_pushBall = 'carState_pushBall'
carState_wait = 'carState_wait'
carState = carState_begin
# variables in control
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:
velocity_control.vel_control(
0, 0.4) # low speed to rotate to front
else:
velocity_control.vel_control(0, -0.4)
print "angle_error", angle_error
rate.sleep()
velocity_control.vel_control(0, 0)
print "Finaly error: ", angle_error
# get ball position:
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
result_dict = detect(net,
meta,
cam_param,
image_np,
isshow=False,
# (position, rotation) = mymove.get_odom()
# print position, rotation
# rate.sleep()
# rospy.spin()
# mymove=point_control.GotoPoint()
rospy.init_node('mc3_main', anonymous=True)
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()
ssr_stage=2
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:#find and catch ball
if move_start==0:
# print(point_control.msg)
# (goal_x, goal_y, goal_z) = point_control.getkey()
# move_start=1
det_flag=True
while not has_rev:
print "wait"
if bdict["football"] is None or len(bdict["football"])<1: # if detection return []
velocity_control.vel_control(0,1)
else:
print "bdict=",bdict
_, goal_x, goal_y = bdict["football"]
goal_z=0
move_start=1
if move_start==1:
has_rev=False
print "xyz",goal_x, goal_y, goal_z
if point_control.PointMove(goal_x, goal_y, goal_z)==4:
move_start=0
if ssr_stage==2:#push ball
det_flag=True
while not has_rev:
pass
# print "stage 2 wait"