forked from gopalmenon/Raspberri-Pi-GoPiGo-Robot-EKF-SLAM
/
SenseLandmarks.py
96 lines (73 loc) · 2.64 KB
/
SenseLandmarks.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
# Constants and functions for sensing obstacles
import numpy
import gopigo
import time
import MoveRobot
#Port that the sbanner is connected to
PORT_A1 = 15
#Scan start, end and step values
SCAN_START = 20
SCAN_END = 160
SCAN_STEP = 10
NUMBER_OF_SCANS = int((SCAN_END - SCAN_START) / SCAN_STEP) + 1
SLEEP_TIME_BETWEEN_STEPS = 0.25
SENSE_LIMIT = 250
SEARCH_LANDMARK_STEP = 20
AVERAGE_DISTANCE_ERROR = 17.92
AVERAGE_ANGLE_ERROR = -11.57
#Robot heading in terms of servo
ROBOT_HEADING_FOR_SERVO = 90
#Scan uncertainty
SENSE_UNCERTAINTY = numpy.matrix([[ 50.2790301042, -5.56573435692, 0.0],
[-5.56573435692, 24.8374625533, 0.0],
[ 0.0, 0.0, 0.0]])
# Make a sweep and return distance to obstacles
def make_sweep():
obstacle_range = numpy.zeros(NUMBER_OF_SCANS)
index = 0
gopigo.enable_servo()
#Scan the vicinity in front
for angle in range(SCAN_START, SCAN_END + SCAN_STEP, SCAN_STEP):
gopigo.servo(angle)
time.sleep(SLEEP_TIME_BETWEEN_STEPS)
distance = gopigo.us_dist(PORT_A1)
#Take into account sense lmits
if distance < SENSE_LIMIT and distance >= 0:
obstacle_range[index] = distance - AVERAGE_DISTANCE_ERROR
else:
obstacle_range[index] = SENSE_LIMIT
index += 1
gopigo.disable_servo()
return obstacle_range
# Return angle to obstacle given bearing array index
def get_angle_to_obstacle(index):
servo_angle = SCAN_START + index * SCAN_STEP
# Convert the servo angle to the robot heading angle
return ROBOT_HEADING_FOR_SERVO - servo_angle -AVERAGE_ANGLE_ERROR
# Return angle and distance to nearest obstacle
def get_nearest_obstacle():
obstacle_range = make_sweep()
nearest_obstacle_index = numpy.argmin(obstacle_range)
if (obstacle_range[nearest_obstacle_index] == SENSE_LIMIT):
no_obstacles_found = True
else:
no_obstacles_found = False
#Look for an obstacle
search_angle = 0
while no_obstacles_found:
# Turn clockwise by the step degrees to check there
search_angle += SEARCH_LANDMARK_STEP
MoveRobot.turn_in_place(SEARCH_LANDMARK_STEP)
# Look for obstacles
obstacle_range = make_sweep()
nearest_obstacle_index = numpy.argmin(obstacle_range)
if (obstacle_range[nearest_obstacle_index] == SENSE_LIMIT):
no_obstacles_found = True
else:
no_obstacles_found = False
# if no obstacle found anywhere, move forward and search again
if no_obstacles_found and search_angle >= MoveRobot.FULL_REVOLUTION_DEGREES :
search_angle = 0
print("Going to move ", MoveRobot.ROBOT_LENGTH_CM)
MoveRobot.go_forward(MoveRobot.ROBOT_LENGTH_CM)
return get_angle_to_obstacle(nearest_obstacle_index), obstacle_range[nearest_obstacle_index]