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 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 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 atSide(self, lidar):
_, minDistance = LidarHelper.shortestDistInRange(
lidar,
self.getGoalAngle() - 2,
self.getGoalAngle() + 2)
return minDistance < 1