def error(self, lidar, zed, imu): minIndex, minDistance = LidarHelper.shortestDistInRange(\ lidar, self.direction() * -(90 + 5), self.direction() * 45) minAngle = LidarHelper.lidarIndexToAngle(minIndex) if minDistance > 2: return 0 offsetX = minDistance * math.cos(math.radians(minAngle + 90 * self.direction())) return self.direction() * (offsetX - GOAL_X)
def lidarCallback(self, data): self.lidar = data.ranges lidar = self.lidar minIndex, minDistance = LidarHelper.shortestDistInRange(lidar, -25, 25) if minDistance < LOOK_FOR_SHORTCUT_LENGTH: angleLeft = self.findHoleAngleLeft(lidar, minIndex) angleRight = self.findHoleAngleRight(lidar, minIndex)
def error(self, lidar, zed, imu): a = .5 b = 1 - a minIndex, minDistance = LidarHelper.shortestDistInRange( lidar, self.getMinAngle(), self.getMaxAngle()) # print('minIndex = %d, minDistance = %f' % (minIndex, minDistance)) return a * self.errorAngle(minIndex) + b * self.errorDist(minDistance)
def findHoleAngle(self, lidar, minIndex, dir): index = minIndex try: #TODO restict ranges a bit more while lidar[index] < HOLE_DEPTH: index += dir startHoleIndex = index while lidar[index] >= HOLE_DEPTH: index += dir endHoleIndex = index holeIndex = self.mid(startHoleIndex, endHoleIndex) return LidarHelper.lidarIndexToAngle(holeIndex) except IndexError: return -1 # It's not on this side
def getPerpDistance(self, lidarPoints, angle): # Perpendicular index = LidarHelper.angleToLidarIndex(angle) vectorDistance = lidarPoints[index] return abs(LidarSteerManager.getXComponentOfVector(vectorDistance, 90 - angle))
def shouldChangeState(self, lidar, zed, imu): LEFT_RANGE = 2 _, minDistance = LidarHelper.shortestDistInRange(\ lidar, self.direction() * -90 - LEFT_RANGE, self.direction() * -90 + LEFT_RANGE) return minDistance < .6
def lidarSomethingIsInFront(self, lidar): FRONT_RANGE = 2 _, minDistance = LidarHelper.shortestDistInRange( lidar, -FRONT_RANGE, FRONT_RANGE) return minDistance < 2.3
def errorAngle(self, minIndex): minAngle = LidarHelper.lidarIndexToAngle(minIndex) # print('minAngle = %f' % minAngle) return (self.getGoalAngle() - minAngle) / abs(self.getMaxAngle() - self.getMinAngle())
def atSide(self, lidar): _, minDistance = LidarHelper.shortestDistInRange( lidar, self.getGoalAngle() - 2, self.getGoalAngle() + 2) return minDistance < 1