NikolajDanger/Roboteksperimentarium
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

✨

Browse Source
This commit is contained in:
NikolajDanger
2022-10-10 13:47:38 +02:00
parent e1e229efc7
commit f547e8e0c3
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
selflocalization/selflocalize.py
View File

@ -155,7 +155,7 @@ def calc_weight(particle, landmark_values):
particle.setWeight(np.product(weights)) particle.setWeight(np.product(weights))
def drive_to_middle(est_pose, arlo): def drive_towards_middle(est_pose, arlo):
middle = np.array([150, 0]) middle = np.array([150, 0])
position = np.array([est_pose.x, est_pose.y]) position = np.array([est_pose.x, est_pose.y])
@ -175,7 +175,7 @@ def drive_to_middle(est_pose, arlo):
arlo.stop() arlo.stop()
arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1) arlo.go_diff(POWER, POWER + RIGHT_WHEEL_OFFSET, 1, 1)
sleep((drive_distance * DRIVE_T)/1000) sleep(0.5)
arlo.stop() arlo.stop()
# Main program # # Main program #
@ -215,8 +215,9 @@ try:
else: else:
cam = camera.Camera(0, 'macbookpro', useCaptureThread = True) cam = camera.Camera(0, 'macbookpro', useCaptureThread = True)
while True: arlo.go_diff(POWER, POWER, 0, 1)
for _ in range(100):
# Move the robot according to user input (only for testing) # Move the robot according to user input (only for testing)
action = cv2.waitKey(10) action = cv2.waitKey(10)
if action == ord('q'): # Quit if action == ord('q'): # Quit
@ -245,6 +246,7 @@ try:
# Draw detected objects # Draw detected objects
cam.draw_aruco_objects(colour) cam.draw_aruco_objects(colour)
else: else:
# No observation - reset weights to uniform distribution # No observation - reset weights to uniform distribution
for p in particles: for p in particles:
@ -252,7 +254,6 @@ try:
particle.add_uncertainty(particles, SIGMA, SIGMA_THETA) particle.add_uncertainty(particles, SIGMA, SIGMA_THETA)
est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose est_pose = particle.estimate_pose(particles) # The estimate of the robots current pose
drive_to_middle(est_pose, arlo)
if showGUI: if showGUI:
# Draw map # Draw map
@ -264,6 +265,9 @@ try:
# Show world # Show world
cv2.imshow(WIN_World, world) cv2.imshow(WIN_World, world)
arlo.stop()
drive_towards_middle(est_pose, arlo)
finally: finally:
# Make sure to clean up even if an exception occurred # Make sure to clean up even if an exception occurred