def main(): global SPIN_RATE global new_pos global oldpos0 global oldpos1 pf = particleFilter(2) # Initialize ROS node rospy.init_node('lab3MoveNode') # Initialize publisher for ur3/command with buffer size of 10 pub_command = rospy.Publisher('ur3/command', command, queue_size=10) sub_position = rospy.Subscriber('ur3/position', position, position_callback) sub_coord = rospy.Subscriber('/coord_center', String, coord_callback) # Check if ROS is ready for operation while (rospy.is_shutdown()): print("ROS is shutdown!") # Initialize the rate to publish to ur3/command loop_rate = rospy.Rate(SPIN_RATE) home_init = lab_invk(new_pos[0], new_pos[1], new_pos[2], new_pos[3]) rospy.loginfo("Moving robot ...\n") move_arm(pub_command, loop_rate, home_init, 4, 4) rospy.loginfo("Home initialization finished!\n") time.sleep(1) rospy.loginfo("Press direction keys to move the robot!") step_size = 0.025 oldpos0 = new_pos[0] oldpos1 = new_pos[1] while not rospy.is_shutdown(): looprun(pf, step_size, pub_command, loop_rate)
def looprun(partfilt, step_sz, cmd, rate): global new_pos global oldpos0 global oldpos1 global t_step global blob_center global xCAM global yCAM #Control command:0 halt, 1 down, 2 right, 3 up, 4 left control = 0 # If not the initial round, generate new samples if (t_step > 0): partfilt.Sample_Motion_Model(control) else: time.sleep(0.2) (xCAM, yCAM) = blob_center_trans(blob_center) # Assign weights to each particles partfilt.Measurement_Model(xCAM, yCAM) # Estimate current position estimatePosition = partfilt.calcPosition() print('Estimated Position: ' + str(estimatePosition)) # Resample the particles partfilt.resampling() # Plot particles partfilt.model.plotParticles(partfilt.particles) # Plot estimated position partfilt.model.plotEstimation(estimatePosition) print( 'Use the arrow keys to command the robot left, right, forward and backward' ) if (t_step > 0): key_result = which_key(getkey()) if (len(key_result) == 1): if (int(key_result) == 3): control = 3 oldpos0 = new_pos[0] oldpos1 = new_pos[1] new_pos[0] -= step_sz elif (int(key_result) == 1): control = 1 oldpos0 = new_pos[0] oldpos1 = new_pos[1] new_pos[0] += step_sz elif (int(key_result) == 2): control = 2 oldpos0 = new_pos[0] oldpos1 = new_pos[1] new_pos[1] += step_sz elif (int(key_result) == 4): control = 4 oldpos0 = new_pos[0] oldpos1 = new_pos[1] new_pos[1] -= step_sz else: control = 0 print(key_result) partfilt.model.map.clear_objects() partfilt.model.run(control) print('Actual Postions: ' + str(partfilt.model.readPosition)) if canmove == 1: new_pos = check_boundary(new_pos) # print(new_pos) new_dest = lab_invk(new_pos[0], new_pos[1], new_pos[2], new_pos[3]) # print(new_dest) move_arm(cmd, rate, new_dest, 4, 4) rospy.loginfo("Destination reached!") else: new_pos[0] = oldpos0 # x new_pos[1] = oldpos1 # y t_step = t_step + 1 print('\n\nSleeping ...') time.sleep(0.5) (xCAM, yCAM) = blob_center_trans(blob_center) print('\n\n')