def test_difference_d(self):
"""Tests the difference calculation between two headings."""
self.assertAlmostEqual(Telemetry.difference_d(359.0, 0.0), 1.0)
self.assertAlmostEqual(Telemetry.difference_d(0.0, 1.0), 1.0)
self.assertAlmostEqual(Telemetry.difference_d(359.0, 1.0), 2.0)
self.assertAlmostEqual(Telemetry.difference_d(360.0, 365.0), 5.0)
self.assertAlmostEqual(Telemetry.difference_d(-355.0, 365.0), 0.0)
def test_difference_d(self):
"""Tests the difference calculation between two headings."""
self.assertAlmostEqual(Telemetry.difference_d(359.0, 0.0), 1.0)
self.assertAlmostEqual(Telemetry.difference_d(0.0, 1.0), 1.0)
self.assertAlmostEqual(Telemetry.difference_d(359.0, 1.0), 2.0)
self.assertAlmostEqual(Telemetry.difference_d(360.0, 365.0), 5.0)
self.assertAlmostEqual(Telemetry.difference_d(-355.0, 365.0), 0.0)
def _run_course_iterator(self):
"""Runs a single iteration of the course navigation loop."""
while not self._waypoint_generator.done():
telemetry = self._telemetry.get_data()
current_waypoint = self._waypoint_generator.get_current_waypoint(
telemetry['x_m'],
telemetry['y_m']
)
distance_m = math.sqrt(
(telemetry['x_m'] - current_waypoint[0]) ** 2
+ (telemetry['y_m'] - current_waypoint[1]) ** 2
)
self._logger.debug(
'Distance to goal {waypoint}: {distance}'.format(
waypoint=[round(i, 3) for i in current_waypoint],
distance=round(distance_m, 3),
)
)
# We let the waypoint generator tell us if a waypoint has been
# reached so that it can do fancy algorithms, like "rabbit chase"
if self._waypoint_generator.reached(
telemetry['x_m'],
telemetry['y_m']
):
self._logger.info(
'Reached {}'.format(
[round(i, 3) for i in current_waypoint]
)
)
self._waypoint_generator.next()
continue
if distance_m > 10.0 or distance_m / telemetry['speed_m_s'] > 2.0:
throttle = 1.0
else:
throttle = 0.5
degrees = Telemetry.relative_degrees(
telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1]
)
heading_d = telemetry['heading_d']
self._logger.debug(
'My heading: {my_heading}, goal heading: {goal_heading}'.format(
my_heading=round(heading_d, 3),
goal_heading=round(degrees, 3),
)
)
diff_d = Telemetry.difference_d(degrees, heading_d)
if diff_d < 10.0:
# We want to keep turning until we pass the point
is_left = Telemetry.is_turn_left(heading_d, degrees)
while diff_d < 10.0:
yield True
telemetry = self._telemetry.get_data()
degrees = Telemetry.relative_degrees(
telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1]
)
heading_d = telemetry['heading_d']
diff_d = Telemetry.difference_d(degrees, heading_d)
if self._waypoint_generator.reached(
telemetry['x_m'],
telemetry['y_m']
):
self._logger.info(
'Reached {}'.format(
[round(i, 3) for i in current_waypoint]
)
)
self._waypoint_generator.next()
break
if Telemetry.is_turn_left(heading_d, degrees) != is_left:
break
self._driver.drive(throttle, 0.0)
yield True
continue
# No sharp turns when we are close, to avoid hard swerves
elif distance_m < 3.0:
turn = 0.25
elif diff_d > 90.0:
turn = 1.0
elif diff_d > 45.0:
turn = 0.5
else:
turn = 0.25
# Turning while going fast causes the car to roll over
if telemetry['speed_m_s'] > 7.0 or throttle >= 0.75:
turn = max(turn, 0.25)
elif telemetry['speed_m_s'] > 4.0 or throttle >= 0.5:
turn = max(turn, 0.5)
if Telemetry.is_turn_left(heading_d, degrees):
turn = -turn
self._driver.drive(throttle, turn)
yield True
self._logger.info('No waypoints, stopping')
self._driver.drive(0.0, 0.0)
self.stop()
yield False
def _run_course_iterator(self):
"""Runs a single iteration of the course navigation loop."""
while not self._waypoint_generator.done():
telemetry = self._telemetry.get_data()
current_waypoint = self._waypoint_generator.get_current_waypoint(
telemetry['x_m'], telemetry['y_m'])
distance_m = math.sqrt((telemetry['x_m'] -
current_waypoint[0])**2 +
(telemetry['y_m'] - current_waypoint[1])**2)
self._logger.debug(
'Distance to goal {waypoint}: {distance}'.format(
waypoint=[round(i, 3) for i in current_waypoint],
distance=round(distance_m, 3),
))
# We let the waypoint generator tell us if a waypoint has been
# reached so that it can do fancy algorithms, like "rabbit chase"
if self._waypoint_generator.reached(telemetry['x_m'],
telemetry['y_m']):
self._logger.info('Reached {}'.format(
[round(i, 3) for i in current_waypoint]))
self._waypoint_generator.next()
continue
if distance_m > 10.0 or distance_m / telemetry['speed_m_s'] > 2.0:
throttle = 1.0
else:
throttle = 0.5
degrees = Telemetry.relative_degrees(telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1])
heading_d = telemetry['heading_d']
self._logger.debug(
'My heading: {my_heading}, goal heading: {goal_heading}'.
format(
my_heading=round(heading_d, 3),
goal_heading=round(degrees, 3),
))
diff_d = Telemetry.difference_d(degrees, heading_d)
if diff_d < 10.0:
# We want to keep turning until we pass the point
is_left = Telemetry.is_turn_left(heading_d, degrees)
while diff_d < 10.0:
yield True
telemetry = self._telemetry.get_data()
degrees = Telemetry.relative_degrees(
telemetry['x_m'], telemetry['y_m'],
current_waypoint[0], current_waypoint[1])
heading_d = telemetry['heading_d']
diff_d = Telemetry.difference_d(degrees, heading_d)
if self._waypoint_generator.reached(
telemetry['x_m'], telemetry['y_m']):
self._logger.info('Reached {}'.format(
[round(i, 3) for i in current_waypoint]))
self._waypoint_generator.next()
break
if Telemetry.is_turn_left(heading_d, degrees) != is_left:
break
self._driver.drive(throttle, 0.0)
yield True
continue
# No sharp turns when we are close, to avoid hard swerves
elif distance_m < 3.0:
turn = 0.25
elif diff_d > 90.0:
turn = 1.0
elif diff_d > 45.0:
turn = 0.5
else:
turn = 0.25
# Turning while going fast causes the car to roll over
if telemetry['speed_m_s'] > 7.0 or throttle >= 0.75:
turn = max(turn, 0.25)
elif telemetry['speed_m_s'] > 4.0 or throttle >= 0.5:
turn = max(turn, 0.5)
if Telemetry.is_turn_left(heading_d, degrees):
turn = -turn
self._driver.drive(throttle, turn)
yield True
self._logger.info('No waypoints, stopping')
self._driver.drive(0.0, 0.0)
self.stop()
yield False
