-
Notifications
You must be signed in to change notification settings - Fork 12
/
Copy pathradarGuidance.py
executable file
·143 lines (120 loc) · 3.36 KB
/
radarGuidance.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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
#!/usr/bin/env python
# -*- coding: utf-8 -*-
""" Benoît Girard
ISIR CNRS/UPMC
01/10/2019
"""
# basic pyfastsim requirements:
from pyfastsim import *
import time
# imports specific to wallFollower
from math import *
# behavioral parameters:
#--------------------------------------
v_fwd = 4.
v_turn = 0.5
# scans used to check wall distances:
angleLMin = 0
angleLMax = 55
angleFMin = 56
angleFMax = 143
angleRMin = 144 #199-55
angleRMax = 199
th_obstacleTooClose = 13
#--------------------------------------
# radarGuidance: orients the robot towars the beacon detected by the radar sensor
# * laserRanges: used to avoid getting stuck in obstacles not detected by the bumpers
# * bumper_l: activation of the left bumper
# * bumper_r: activation of the right bumper
# * radar : the direction sensor indicating the direction of the goal
def radarGuidance(laserRanges, bumper_l, bumper_r,radar,verbose=True):
wallTooCloseL = False
wallTooCloseF = False
wallTooCloseR = False
v = [0,0]
# determine if obstacle too close:
for i in range(len(laserRanges)):
if laserRanges[i] < th_obstacleTooClose:
if i in range(angleLMin,angleLMax):
wallTooCloseL = True
if i in range(angleFMin,angleFMax):
wallTooCloseF = True
if i in range(angleRMin,angleRMax):
wallTooCloseR = True
# Choose policy based on obstacle and beacon direction detection:
if wallTooCloseF:
if verbose:
print('WALL F')
v[0] = -v_fwd
v[1] = -v_fwd
elif bumper_r or wallTooCloseR:
if verbose:
print('WALL R')
v[0] = v_fwd
v[1] = -v_fwd
elif bumper_l or wallTooCloseL:
if verbose:
print('WALL L')
v[0] = -v_fwd
v[1] = v_fwd
elif (7 == radar) :
if verbose:
print('FWD L')
v[0] = v_fwd
v[1] = v_fwd*.8
elif (0 == radar):
if verbose:
print('FWD R')
v[0] = v_fwd*.8
v[1] = v_fwd
# if it is on the left :
elif (6 == radar) or (5 == radar):
if verbose:
print('LEFT')
v[0] = v_fwd
v[1] = v_turn
# if it is on the right :
elif (1 == radar) or (2 == radar):
if verbose:
print('RIGHT')
v[0] = v_turn
v[1] = v_fwd
# if it is behind :
elif (3 == radar) :
if verbose:
print('BEHIND R')
v[0] = -v_fwd
v[1] = v_fwd
elif (4 == radar) :
if verbose:
print('BEHIND L')
v[0] = v_fwd
v[1] = -v_fwd
return v
#--------------------------------------
def main():
settings = Settings('worlds/entonnoir.xml')
env_map = settings.map()
robot = settings.robot()
d = Display(env_map, robot)
for i in range(1000):
d.update()
# get sensory data from the simulation
#------------------------------------
pos = robot.get_pos()
print("Step %d robot pos: x = %f y = %f theta = %f" % (i, pos.x(), pos.y(), pos.theta()))
# get the sensor inputs and prepare them for the radarGuidance() function:
lasers = robot.get_laser_scanners()[0].get_lasers()
laserRanges = []
for l in lasers:
laserRanges.append(l.get_dist())
radar = robot.get_radars()[0].get_activated_slice()
bumperL = robot.get_left_bumper()
bumperR = robot.get_right_bumper()
#------------------------------------
v = radarGuidance(laserRanges,bumperL,bumperR,radar)
robot.move(v[0], v[1], env_map)
time.sleep(0.01)
#--------------------------------------
if __name__ == '__main__':
main()