In this example, after the robot consumes three pucks, all of the pucks and the robot will be randomly repositioned. This is accomplished through the use of truth devices.
In order to use truth devices, you first need to update the world file. Each truth device needs a port number. In the case of a truth device attached to a robot, it can share the robot's port number. However, for truth devices attached to pucks, you need to assign additional unique port numbers. See the example Player/Stage world file below.
You may need to change the path to the includes and files. For example, you may need to change /usr/local/pyrobot/plugins/worlds/ to /usr/local/worlds/. This is a problem in Player/Stage and they are working on it.
# File: FindPucksTruth.world
# Robot and pucks with attached truth devices.
# the resolution of Stage's raytrace model in meters
resolution 0.02
gui
(
size [ 502.000 506.000 ]
origin [5.018 4.950 0]
scale 0.021 # the size of each bitmap pixel in meters
)
bitmap
(
file "/usr/local/pyrobot/plugins/worlds/Stage/rink.pnm.gz"
resolution 0.01
)
define red_puck puck( color "red" )
include "/usr/local/pyrobot/plugins/worlds/Stage/usc_pioneer.inc"
usc_pioneer_gripper( color "green" name "myrobot"
port 6665 pose [ 2.0 2.0 20 ]
truth () )
red_puck( pose [3.3 3.1 0.000] truth ( port 7000 ))
red_puck( pose [4.7 3.8 0.000] truth ( port 7001 ))
red_puck( pose [2.2 4 0.000] truth ( port 7002 ))
red_puck( pose [1.7 6.1 0.000] truth ( port 7003 ))
red_puck( pose [3.9 6.2 0.000] truth ( port 7004 ))
red_puck( pose [6.4 5.8 0.000] truth ( port 7005 ))
Since the pucks will now be active entities, you'll need to link them to a Player. Also, before you can use any device in Player/Stage you must first start it. This only needs to be done once, so is typically done in the constructor for the robot's brain. See the updated constructor shown below.
Each truth device maintains a list of length three containing the x position, y position, and heading of the attached entity. You can get this information using the method get('robot/truth/pose') and you can set this information using the method set('robot/truth/pose', (x, y, h)). Examples of calling these two methods are provided below in the methods statusPucks and randomizePositions.
# File: FindPucksTruth.py
from pyrobot.brain import Brain
from pyrobot.robot.player import *
import time
import random
def randInRange(low, high):
return (random.random() * (high-low)) + low
# Searches for red pucks and tries to grab and consume them.
# After finding three, the pucks are randomly repositioned.
class FindBlobs(Brain):
def setup(self):
self.robot.startDevices(['truth', 'blobfinder', 'gripper'])
self.pucksFound = 0
self.pucks = range(6)
for i in range(6):
self.pucks[i] = PlayerRobot('puck'+str(i), 7000+i)
self.pucks[i].startDevice('truth')
def consumePuck(self):
robot = self.robot
gripper = robot.getDevice("gripper0")
if gripper.isClosed():
gripper.store()
self.pucksFound += 1
print "pucks found:", self.pucksFound
time.sleep(1)
gripper.open()
time.sleep(1)
elif gripper.getBreakBeamState():
robot.move(0, 0)
gripper.close()
else:
robot.move(0.05, 0)
def seekColor(self, channel):
minRange = 0.9
gripper = self.getDevice("gripper0")
if gripper.getBreakBeamState() or gripper.getState():
self.consumePuck()
elif self.collisionImminent('front-all', minRange):
translate, rotate = self.avoid(minRange)
self.move(translate, rotate)
else:
result = self.getClosestBlob(channel)
if result == 'None':
leftSide = min(self.get('robot/range/front-left/value'))
rightSide = min(self.get('robot/range/front-left/value'))
if leftSide < rightSide:
self.move(0.1, -0.5)
else:
self.move(0.1, 0.5)
else:
turnDirection = result[0]
range = result[1]
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 1 when the minimum sonar value from the given location group is
# less than the minRange.
def collisionImminent(self, location, minRange):
if min(self.get('robot/range',location,'value')) < minRange:
return 1
return 0
# 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 on the given color channel.
def getBlobChannel(self, channel):
index = 0
data = self.robot.getDeviceData("blob0")
if channel == 'red':
index = 0
elif channel == 'green':
index = 1
elif channel == 'blue':
index = 2
else:
print "unrecognized color channel"
if len(data) == 0 or len(data[1]) == 0:
return []
else:
return data[1][index]
# Returns a list of the two key features of the blob information for the
# closest blob on the given channel.
# The blob information consists of a list of nine features:
# Index 2: is an integer between 0 and 160 which represents the x position
# of the blob's centeroid. 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.
# Index 8: is an integer representing the range 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 getClosestBlob(self, channel):
all = self.getBlobChannel(channel)
if len(all) == 0:
return 'None'
else:
if all[0][2] < 75:
turnDirection = 1
elif all[0][2] > 85:
turnDirection = -1
else:
turnDirection = 0
return turnDirection, all[0][8]
def step(self):
if (self.pucksFound > 2):
# move pucks to random locations after three are found
self.randomizePositions()
self.statusPucks()
self.pucksFound = 0
else:
self.seekColor('red')
def randomizePositions(self):
for object in self.pucks + [self.robot]:
x = randInRange(1500, 6500)
y = randInRange(2000, 6000)
h = randInRange(0, 359)
object.getDevice("truth0").setPose(x, y, h)
def statusPucks(self):
for i in range(len(self.pucks)):
print i, self.pucks[i].getDevice("truth0").getPose(),
print "\n"
def INIT(engine):
return FindBlobs('FindBlobs', engine)
To execute this example do:
pyro -s Stage -w FindPucksTruth.world -r Player1 -b FindPucksTruth.py
Return to PyroModuleMultirobot.
