def goToG( self, target, finishRadius, backward=False, angleThreshold = math.radians(10), angularSpeed = math.radians(10) ):
"generator to reach given destination with given precision"
headingOffset = backward and math.radians(180) or 0.0
prevPose = pose = self.robot.localisation.pose()
angularSpeed = self.restrictedTurn(angularSpeed)
while distance( pose, target ) > finishRadius:
angleDiff = normalizeAnglePIPI( angleTo(pose, target) - pose[2] - headingOffset)
# print angleDiff
if math.fabs( angleDiff ) > angleThreshold:
for cmd in self.turnG( angleDiff, angleThreshold = angleThreshold ):
yield cmd
pose = self.robot.localisation.pose()
if distance( pose, target ) < self.stopDistance():
break
angleDiff = normalizeAnglePIPI( angleTo(pose, target) - pose[2] - headingOffset)
if math.fabs( angleDiff ) > angleThreshold / 2.0:
turn = angularSpeed if angleDiff > 0 else -angularSpeed
else:
turn = math.fabs(angleDiff / angleThreshold / 2.0) * (angularSpeed if angleDiff > 0 else -angularSpeed)
speed = self.restrictedSpeed(turn)
if backward:
speed *= -1
yield ( speed, turn )
for cmd in self.stopG():
yield cmd
def followPolyLineG(self,
pts,
stopDistance=0.1,
angleThreshold=math.radians(20),
turnScale=4.0,
offsetSpeed=math.radians(20),
offsetDistance=0.03):
for a, b in zip(pts[:-1], pts[1:]):
print "--- follow (%0.2f,%0.2f) -> (%0.2f,%0.2f) ---" % (
a[0], a[1], b[0], b[1])
pose = self.robot.localisation.pose()
angleDiff = normalizeAnglePIPI(angleTo(pose, b) - pose[2])
if math.fabs(angleDiff) > angleThreshold:
for cmd in self.turnG(angleDiff,
angleThreshold=angleThreshold):
yield cmd
line = Line(a, b)
for cmd in self.followLineG(line,
stopDistance=stopDistance,
turnScale=turnScale,
offsetSpeed=offsetSpeed,
offsetDistance=offsetDistance):
yield cmd
for cmd in self.stopG():
yield cmd
def followLineG(self,
line,
stopDistance=0.0,
turnScale=4.0,
offsetSpeed=math.radians(20),
offsetDistance=0.03):
'''experimental generator - to replace orig function
line ... A line to follow.
stopDistance ... The robot stops when closer than this to the endpoint. [m]
turnScale ... Magic parameter for the rotational speed. [scaling factor]
offsetSpeed ... This extra rotational speed is added when the robot is too far from the line. [rad/s]
offsetDistance ... When the robot is further than this from the line, some extra correction may be needed. [m]
'''
while line.distanceToFinishLine(
self.robot.localisation.pose()) > stopDistance:
diff = normalizeAnglePIPI(line.angle -
self.robot.localisation.pose()[2])
signedDistance = line.signedDistance(
self.robot.localisation.pose()) + self.centerOffset
# print "deg %.1f" %( math.degrees(diff),), "dist=%0.3f" % (signedDistance,)
if math.fabs(signedDistance) > offsetDistance:
if signedDistance < 0:
diff += offsetSpeed
else:
diff -= offsetSpeed
turn = self.restrictedTurn(turnScale * diff)
speed = self.restrictedSpeed(turn)
yield speed, turn
def cmdDaisy( self, pose, goal, maxSpeed ):
angleDiff = normalizeAnglePIPI( angleTo(pose, goal)-pose[2] )
angularSpeed = self.maxAngularSpeed
if angleDiff > math.pi/2.0:
return ( 0.0, angularSpeed )
elif angleDiff < -math.pi/2.0:
return ( 0.0, -angularSpeed )
return math.cos(angleDiff)*maxSpeed, math.sin(angleDiff)*angularSpeed
def cmdDaisy(self, pose, goal, maxSpeed):
angleDiff = normalizeAnglePIPI(angleTo(pose, goal) - pose[2])
angularSpeed = self.maxAngularSpeed
if angleDiff > math.pi / 2.0:
return (0.0, angularSpeed)
elif angleDiff < -math.pi / 2.0:
return (0.0, -angularSpeed)
return math.cos(angleDiff) * maxSpeed, math.sin(
angleDiff) * angularSpeed
def followPolyLineG( self, pts, stopDistance = 0.1, angleThreshold = math.radians(20), turnScale = 4.0, offsetSpeed = math.radians(20), offsetDistance = 0.03 ):
for a,b in zip(pts[:-1],pts[1:]):
print "--- follow (%0.2f,%0.2f) -> (%0.2f,%0.2f) ---" % ( a[0], a[1], b[0], b[1] )
pose = self.robot.localisation.pose()
angleDiff = normalizeAnglePIPI( angleTo(pose, b) - pose[2])
if math.fabs( angleDiff ) > angleThreshold:
for cmd in self.turnG( angleDiff, angleThreshold = angleThreshold ):
yield cmd
line = Line(a,b)
for cmd in self.followLineG( line, stopDistance = stopDistance, turnScale = turnScale, offsetSpeed = offsetSpeed, offsetDistance = offsetDistance ):
yield cmd
for cmd in self.stopG():
yield cmd
def goToG(self,
target,
finishRadius,
backward=False,
angleThreshold=math.radians(10),
angularSpeed=math.radians(10)):
"generator to reach given destination with given precision"
headingOffset = backward and math.radians(180) or 0.0
prevPose = pose = self.robot.localisation.pose()
angularSpeed = self.restrictedTurn(angularSpeed)
while distance(pose, target) > finishRadius:
angleDiff = normalizeAnglePIPI(
angleTo(pose, target) - pose[2] - headingOffset)
# print angleDiff
if math.fabs(angleDiff) > angleThreshold:
for cmd in self.turnG(angleDiff,
angleThreshold=angleThreshold):
yield cmd
pose = self.robot.localisation.pose()
if distance(pose, target) < self.stopDistance():
break
angleDiff = normalizeAnglePIPI(
angleTo(pose, target) - pose[2] - headingOffset)
if math.fabs(angleDiff) > angleThreshold / 2.0:
turn = angularSpeed if angleDiff > 0 else -angularSpeed
else:
turn = math.fabs(angleDiff / angleThreshold / 2.0) * (
angularSpeed if angleDiff > 0 else -angularSpeed)
speed = self.restrictedSpeed(turn)
if backward:
speed *= -1
yield (speed, turn)
for cmd in self.stopG():
yield cmd
def followLineG( self, line, stopDistance = 0.0, turnScale = 4.0, offsetSpeed = math.radians(20), offsetDistance = 0.03 ):
'''experimental generator - to replace orig function
line ... A line to follow.
stopDistance ... The robot stops when closer than this to the endpoint. [m]
turnScale ... Magic parameter for the rotational speed. [scaling factor]
offsetSpeed ... This extra rotational speed is added when the robot is too far from the line. [rad/s]
offsetDistance ... When the robot is further than this from the line, some extra correction may be needed. [m]
'''
while line.distanceToFinishLine( self.robot.localisation.pose() ) > stopDistance:
diff = normalizeAnglePIPI( line.angle - self.robot.localisation.pose()[2] );
signedDistance = line.signedDistance( self.robot.localisation.pose() ) + self.centerOffset
# print "deg %.1f" %( math.degrees(diff),), "dist=%0.3f" % (signedDistance,)
if math.fabs( signedDistance ) > offsetDistance:
if signedDistance < 0:
diff += offsetSpeed
else:
diff -= offsetSpeed
turn = self.restrictedTurn(turnScale * diff)
speed = self.restrictedSpeed(turn)
yield speed, turn
