def __init__(self, robotName): self.gazeboModelStater = GazeboModelStater() self.path = [] self.cPathI = 0 self.lookAhead = 1.5 self.errorMultiplier = 1 self.robotName = robotName
def __init__(self, robotName): self.gazeboModelStater = GazeboModelStater() self.path = [] self.lookAhead = 1.5 self.errorMultiplier = 1 self.currentSegment = (-1,-1) self.robotName = robotName
class DroneController(): def __init__(self, robotName): self.gazeboModelStater = GazeboModelStater() self.pathPoses = [] self.pathNames = [] self.lookAhead = 1.5 self.errorMultiplier = 1 self.currentSegment = (-1,-1) self.robotName = robotName def addUpdateCallback(self, callback): self.gazeboModelStater.addUpdateCallback(callback) def setPath(self, path): self.pathNames = path self.pathPoses = getPathFromRings(self.gazeboModelStater, self.robotName, path) def getDroneState(self): return self.gazeboModelStater.get(self.robotName) def getClosestLineSegment(self, p): currentDist, currentProj = distToSegment2(self.pathPoses[self.currentSegment[0]],self.pathPoses[self.currentSegment[1]],p) for i in range(2): #start at the current position and look forward index1 = self.addToIndex(self.currentSegment[0], i) index2 = self.addToIndex(index1, 1) a = self.pathPoses[index1] b = self.pathPoses[index2] distance, projected = distToSegment2(a,b,p) if distance < currentDist: return (index1,index2), distance, projected return self.currentSegment, currentDist, currentProj def isForward(self, yaw, currentSegment): a = self.pathPoses[currentSegment[0]] b = self.pathPoses[currentSegment[1]] dir = [math.cos(yaw), math.sin(yaw)] dp = dot(dir,sub(b,a)) if dp > 0: return True else: return False def getControlInfo(self): currentPose, currentTwist = self.getDroneState() if not currentPose or not self.pathPoses: return None, None, None, None pos = [currentPose.position.x, currentPose.position.y] pitch, roll, yaw = euler_from_quaternion([currentPose.orientation.x, currentPose.orientation.y, currentPose.orientation.z, currentPose.orientation.w]) #calculate where we are on the path, and where we should be currentSegment, posErr, projected = self.getClosestLineSegment(pos) self.currentSegment = currentSegment #are we facing forward isForward = self.isForward(yaw, currentSegment) #calculate where we should drive too i = currentSegment[0] j = currentSegment[1] a = self.pathPoses[i] b = self.pathPoses[j] #if isForward: dirMult = 1 #always drive forward #else: # dirMult = -1 remainingLookAhead = self.lookAhead while remainingLookAhead > 0.01: follow = add(projected,scale(normalize(sub(b,a)),dirMult*remainingLookAhead)) remainingLookAhead, projected = distToSegment2(a,b,follow) i = self.addToIndex(i, dirMult) j = self.addToIndex(j, dirMult) a = self.pathPoses[i] b = self.pathPoses[j] positionError = sub(follow,pos) targetYaw = math.atan2(positionError[1], positionError[0]) angleError = targetYaw - yaw while angleError > math.pi: angleError -= 2*math.pi while angleError < -math.pi: angleError += 2*math.pi angleError *= self.errorMultiplier segmentNames = (self.pathNames[currentSegment[0]], self.pathNames[currentSegment[1]]) return angleError, posErr, segmentNames, isForward def addToIndex(self, index, add): newi = index + add if newi < 0: newi += len(self.pathPoses) else: newi = newi % len(self.pathPoses) return newi
class DroneController(): def __init__(self, robotName): self.gazeboModelStater = GazeboModelStater() self.path = [] self.cPathI = 0 self.lookAhead = 1.5 self.errorMultiplier = 1 self.robotName = robotName def addUpdateCallback(self, callback): self.gazeboModelStater.addUpdateCallback(callback) def setPath(self, path): self.path = getPathFromRings(self.gazeboModelStater, self.robotName, path) def getDroneState(self): return self.gazeboModelStater.get(self.robotName) def getPathIndex(self): return self.cPathI def getPositionError(self): currentPose, currentTwist = self.gazeboModelStater.get(self.robotName) if not currentPose: return 0 robotCoord = [currentPose.position.x, currentPose.position.y] #calculate path index while True: indexback = self.subIndex(self.cPathI) index = self.cPathI indexfront = self.addIndex(self.cPathI) if isPassed(robotCoord, self.path[index], self.path[indexback], self.path[indexfront]): self.cPathI = self.addIndex(self.cPathI) else: break cPathI1 = self.addIndex(self.cPathI) dirVect = normalize(add(self.path[cPathI1], neg(self.path[self.cPathI]))) projection = getProjection(robotCoord, self.path[self.cPathI], dirVect) posError = math.sqrt(distance2(robotCoord, projection)) return posError def getAngleError(self): currentPose, currentTwist = self.gazeboModelStater.get(self.robotName) if not currentPose: return 0 robotCoord = [currentPose.position.x, currentPose.position.y] quaternionArray = [currentPose.orientation.x, currentPose.orientation.y, currentPose.orientation.z, currentPose.orientation.w] roll, pitch, robotYaw = euler_from_quaternion(quaternionArray) #calculate path index while True: indexback = self.subIndex(self.cPathI) index = self.cPathI indexfront = self.addIndex(self.cPathI) if isPassed(robotCoord, self.path[index], self.path[indexback], self.path[indexfront]): self.cPathI = self.addIndex(self.cPathI) else: break cPathI1 = self.addIndex(self.cPathI) dirVect = normalize(add(self.path[cPathI1], neg(self.path[self.cPathI]))) projection = getProjection(robotCoord, self.path[self.cPathI], dirVect) followPoint = add(projection, scale(dirVect, self.lookAhead)) #ensure follow point remains on path, could be adding lookAhead directs it away from next path coords followI = self.cPathI while True: index1 = self.addIndex(followI) segLen2 = distance2(self.path[followI], self.path[index1]) followLen2 = distance2(self.path[followI], followPoint) if followLen2 > segLen2: #should place it to the next segment index2 = self.addIndex(index1) extraLen = math.sqrt(followLen2) - math.sqrt(segLen2) dirVect = normalize(add(self.path[index2], neg(self.path[index1]))) followPoint = add(self.path[index1], scale(dirVect, extraLen)) followI = index1 else: break positionError = add(neg(robotCoord), followPoint) targetYaw = math.atan2(positionError[1], positionError[0]) angleError = targetYaw - robotYaw while angleError > math.pi: angleError -= 2*math.pi while angleError < -math.pi: angleError += 2*math.pi return angleError * self.errorMultiplier def subIndex(self, index): newi = index - 1 if newi < 0: newi = newi + len(self.path) return newi def addIndex(self, index): newi = (index + 1) % len(self.path) return newi
class DroneController(): def __init__(self, robotName): self.gazeboModelStater = GazeboModelStater() self.path = [] self.lookAhead = 1.5 self.errorMultiplier = 1 self.currentSegment = (-1,-1) self.robotName = robotName def addUpdateCallback(self, callback): self.gazeboModelStater.addUpdateCallback(callback) def setPath(self, path): self.path = getPathFromRings(self.gazeboModelStater, self.robotName, path) def getDroneState(self): return self.gazeboModelStater.get(self.robotName) def getClosestLineSegment(self, p): closestDist = 99999 closestSegment = (-1,-1) closestProjected = [0,0] shortestJump = 999999 for i in range(len(self.path)): j = self.addToIndex(i,1) a = self.path[i] b = self.path[j] jump = abs(i-self.currentSegment[0]) jumpFromLastSegment = min(jump, abs(jump-len(self.path))) distance, projected = distToSegment2(a,b,p) if distance < closestDist: if closestDist-distance > 1 or jumpFromLastSegment < shortestJump: closestDist = distance closestSegment = (i,j) closestProjected = projected shortestJump = jumpFromLastSegment return closestSegment, closestDist, closestProjected def isForward(self, yaw, currentSegment): a = self.path[currentSegment[0]] b = self.path[currentSegment[1]] dir = [math.cos(yaw), math.sin(yaw)] dp = dot(dir,sub(b,a)) if dp > 0: return True else: return False def getControlInfo(self): currentPose, currentTwist = self.gazeboModelStater.get(self.robotName) if not currentPose: return 0 pos = [currentPose.position.x, currentPose.position.y] pitch, roll, yaw = euler_from_quaternion([currentPose.orientation.x, currentPose.orientation.y, currentPose.orientation.z, currentPose.orientation.w]) #calculate where we are on the path, and where we should be currentSegment, posErr, projected = self.getClosestLineSegment(pos) self.currentSegment = currentSegment #are we facing forward isForward = self.isForward(yaw, currentSegment) #calculate where we should drive too i = currentSegment[0] j = currentSegment[1] a = self.path[i] b = self.path[j] if isForward: dirMult = 1 else: dirMult = -1 remainingLookAhead = self.lookAhead while remainingLookAhead > 0.01: follow = add(projected,scale(normalize(sub(b,a)),dirMult*remainingLookAhead)) remainingLookAhead, projected = distToSegment2(a,b,follow) i = self.addToIndex(i, dirMult) j = self.addToIndex(j, dirMult) a = self.path[i] b = self.path[j] positionError = sub(follow,pos) targetYaw = math.atan2(positionError[1], positionError[0]) angleError = targetYaw - yaw while angleError > math.pi: angleError -= 2*math.pi while angleError < -math.pi: angleError += 2*math.pi angleError *= self.errorMultiplier return angleError, posErr, currentSegment, isForward def addToIndex(self, index, add): newi = index + add if newi < 0: newi += len(self.path) else: newi = newi % len(self.path) return newi
