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"