| 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
|
# OUT_OF_DATE! THIS USES THE OLD PLAYER INTERNAL BLOB DATA STRUCTURES
from pyrobot.brain import Brain
# Seeks out red blobs
class FindBlobs(Brain):
def setup(self):
self.startDevice('blobfinder')
def destroy(self):
self.removeDevice('blobfinder')
def step(self):
self.seekColor('red')
def seekColor(self, color):
minRange = 0.9
if self.collisionImminent('front-all', minRange):
translate, rotate = self.avoid(minRange)
self.move(translate, rotate)
else:
result = self.getLargestBlob(color)
if result == None:
leftSide = min([s.distance() for s in self.robot.range["front-left"]])
rightSide = min([s.distance() for s in self.robot.range["front-right"]])
if leftSide < rightSide:
self.move(0.1, -0.5)
else:
self.move(0.1, 0.5)
else:
(turnDirection, distance) = result
if turnDirection > 0:
self.move(0.1, 0.15)
elif turnDirection < 0:
self.move(0.1, -0.15)
else:
self.move(0.2, 0)
# Returns True when the minimum sonar value from the given location
# group is less than the minRange.
def collisionImminent(self, location, minRange):
return min([s.distance() for s in self.robot.range[location]]) < minRange
# Returns translate and rotate values for avoiding obstacles.
def avoid(self, minRange):
if self.collisionImminent('front', minRange):
return 0, -0.3
elif self.collisionImminent('front-left', minRange):
return 0, -0.3
elif self.collisionImminent('front-right', minRange):
return 0, 0.3
else:
return 0.2, 0
# Returns a list of all blobs of the given color.
def getBlobs(self, color):
data = self.robot.blobfinder.data
(width, height) = data[0]
colorChannels = data[1]
if color == 'red':
index = 0
elif color == 'green':
index = 1
elif color == 'blue':
index = 2
else:
raise ValueError, 'unrecognized color channel: %s' % color
blobs = colorChannels[index]
return blobs
# Returns the direction (+1, -1, or 0) to the largest blob of the
# specified color along with the distance to the blob. The blob
# information consists of a list of nine features:
# (colorCode, area, xc, yc, x0, x1, y0, y1, distance)
# xc is an integer between 0 and 160 which represents the x position of
# the blob's center. A value of 0 indicates that the blob is at the
# farthest left location, a value of 80 indicates that the blob is
# centered, and a value of 160 indicates that the blob is at the farthest
# right location. Distance is an integer representing the distance to the
# blob. When using a Pioneer robot with a gripper, if the blob is
# centered, then a range of 380 indicates that it is within the grasp of
# the gripper.
def getLargestBlob(self, color):
allBlobs = self.getBlobs(color)
if len(allBlobs) == 0:
return None
else:
largestBlob = allBlobs[0]
(colorCode, area, xc, yc, x0, x1, y0, y1, distance) = largestBlob
if xc < 75:
turnDirection = +1
elif xc > 85:
turnDirection = -1
else:
turnDirection = 0
return (turnDirection, distance)
def INIT(engine):
return FindBlobs('FindBlobs', engine) |
