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