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