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')
