def createVFH(verbose=False):
return VFH(robot,
laserFOV=fov,
blockingDistance=0.,
safetyDistance=0.10,
maxRange=1.5,
turnRadius=0.5,
binSize=binSize,
verbose=verbose)
def goToVfhDigit(self, digit, timeout, info=None):
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
RETREAT = 0.2 # A retreated distance after endurance times out. [m]
pathFinder = LaserPath(verbose=False)
self.robot.addExtension(pathFinder.updateExtension, "PATH")
turnRadius = None #1.2
# vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
vfh = VFH(self.robot,
blockingDistance=0.2,
safetyDistance=0.3,
maxRange=1.0,
turnRadius=None,
verbose=False)
self.robot.addExtension(vfh.updateExtension, "VHF")
# Navigate roughly in the suggested direction, avoiding obstacles.
prevDir = 0.0
waypoint = self.cam2waypoint(self.robot.localisation.pose(), info)
goalDir = self.waypoint2dir(self.robot.localisation.pose(), waypoint)
print "GOAL DIR", goalDir
oldCam = self.robot.cameraData
countImages = 0
countVerified = 0
startTime = self.robot.time
happyTime = self.robot.time
while self.robot.time < startTime + timeout:
if oldCam != self.robot.cameraData:
oldCam = self.robot.cameraData
countImages += 1
if self.robot.cameraData and self.robot.cameraData[0]:
info = None
for d in parseCameraData(self.robot.cameraData):
# TODO switch to goToDigit for large enough number
if d[0] == digit:
info = d
if info != None:
countVerified += 1
waypoint = self.cam2waypoint(
self.robot.localisation.pose(), info)
goalDir = self.waypoint2dir(
self.robot.localisation.pose(), waypoint)
print "GOAL DIR", goalDir, oldCam, info, digit
dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
if dir is None or not pathFinder.isTraversable(dir):
strategy = zeroCmd()
prevDir = 0.0
self.robot.toDisplay = 'HH'
else:
pose = self.robot.localisation.pose()
goal = combinedPose((pose[0], pose[1], pose[2] + dir),
(1.0, 0, 0))
strategy = self.driver.goToG(goal,
TOLERATED_MISS,
angleThreshold=ANGULAR_THRESHOLD,
angularSpeed=ANGULAR_SPEED)
prevDir = dir
self.robot.toDisplay = 'OK'
happyTime = self.robot.time
if self.robot.time - happyTime > ENDURANCE:
break # does not have sense to wait - try to turn somewhere else ...
# We are stuck for too long. Let's try to retreat a bit:
# 1) to show we are alive,
# 2) to hope the robot gets out of the blocked state.
# Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
pose = self.robot.localisation.pose()
goal = combinedPose(pose, (-(RETREAT + TOLERATED_MISS), 0, 0))
strategy = self.driver.goToG(goal,
TOLERATED_MISS,
backward=True,
angleThreshold=ANGULAR_THRESHOLD,
angularSpeed=ANGULAR_SPEED)
self.robot.toDisplay = '\/'
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
happyTime = self.robot.time
continue # TODO maybe replace by return -> give up
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
pose = self.robot.localisation.pose()
# isThere = math.hypot(pose[0] - lastX, pose[1] - lastY) <= TOLERATED_MISS
# if isThere or pathFinder.newData:
if pathFinder.newData:
pathFinder.newData = False
break
self.robot.removeExtension("VHF")
self.robot.removeExtension("PATH")
def goVfh(self, timeout, maxDist=None):
print "goVfh", maxDist
TOLERATED_MISS = 0.2 # [m]
ANGULAR_THRESHOLD = math.radians(20) # [rad]
ANGULAR_SPEED = math.radians(60) # [rad/s]
ENDURANCE = 3 # Amount of time the robot remains blocked before timing out and moving backwards. [s]
RETREAT = 0.2 # A retreated distance after endurance times out. [m]
pathFinder = LaserPath(verbose=False)
self.robot.addExtension(pathFinder.updateExtension, "PATH")
turnRadius = None #1.2
# vfh = VFH(self.robot, blockingDistance = 0.35, safetyDistance = 0.6, maxRange = 1.4, turnRadius = turnRadius, verbose = False)
vfh = VFH(self.robot,
blockingDistance=0.2,
safetyDistance=0.3,
maxRange=1.0,
turnRadius=None,
verbose=False)
self.robot.addExtension(vfh.updateExtension, "VHF")
# Navigate roughly in the suggested direction, avoiding obstacles.
prevDir = 0.0
goalDir = 0.0
startTime = self.robot.time
happyTime = self.robot.time
distTraveled = 0.0
while self.robot.time < startTime + timeout:
if maxDist is not None and distTraveled > maxDist:
print "goVfh - maxDist reached"
break
dir = vfh.navigate(goalDir, prevDir, pathFinder.obstacles)
if dir is None or not pathFinder.isTraversable(dir):
strategy = zeroCmd()
prevDir = 0.0
self.robot.toDisplay = 'HH'
else:
pose = self.robot.localisation.pose()
goal = combinedPose((pose[0], pose[1], pose[2] + dir),
(1.0, 0, 0))
strategy = self.driver.goToG(goal,
TOLERATED_MISS,
angleThreshold=ANGULAR_THRESHOLD,
angularSpeed=ANGULAR_SPEED)
prevDir = dir
self.robot.toDisplay = 'OK'
happyTime = self.robot.time
if self.robot.time - happyTime > ENDURANCE:
print "ENDURANCE!"
break # does not have sense to wait - try to turn somewhere else ...
# We are stuck for too long. Let's try to retreat a bit:
# 1) to show we are alive,
# 2) to hope the robot gets out of the blocked state.
# Note: p(bumping backward) <= p(being disqualified for not moving) == 1.0
pose = self.robot.localisation.pose()
goal = combinedPose(pose, (-(RETREAT + TOLERATED_MISS), 0, 0))
strategy = self.driver.goToG(goal,
TOLERATED_MISS,
backward=True,
angleThreshold=ANGULAR_THRESHOLD,
angularSpeed=ANGULAR_SPEED)
self.robot.toDisplay = '\/'
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
distTraveled += self.robot.lastDistStep
happyTime = self.robot.time
continue # TODO maybe replace by return -> give up
for (speed, angularSpeed) in strategy:
self.robot.setSpeedPxPa(speed, angularSpeed)
self.robot.update()
distTraveled += self.robot.lastDistStep
pose = self.robot.localisation.pose()
if pathFinder.newData:
pathFinder.newData = False
break
self.robot.removeExtension("VHF")
self.robot.removeExtension("PATH")
print "... traveled", distTraveled, "time", self.robot.time - startTime
