def _unstuck_yourself_iterator(self, seconds):
"""Commands the car to reverse and try to get off an obstacle."""
# The ESC requires us to send neutral throttle for a bit, then send
# reverse, then neutral, then reverse again (which will actually drive
# the car in reverse)
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_1_S:
self._driver.drive(0.0, 0.0)
yield True
start = time.time()
while time.time() < start + self.REVERSE_TIME_S:
self._driver.drive(-0.5, 0.0)
yield True
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_2_S:
self._driver.drive(0.0, 0.0)
yield True
telemetry = self._telemetry.get_data()
heading_d = telemetry['heading_d']
current_waypoint = self._waypoint_generator.get_current_waypoint(
telemetry['x_m'],
telemetry['y_m']
)
degrees = Telemetry.relative_degrees(
telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1]
)
is_left = Telemetry.is_turn_left(heading_d, degrees)
if is_left is None:
turn_direction = 1.0 if random.randint(0, 1) == 0 else -1.0
elif is_left:
turn_direction = 1.0 # Reversed because we're driving in reverse
else:
turn_direction = -1.0
start = time.time()
while time.time() < start + seconds:
self._driver.drive(-.5, turn_direction)
yield True
# Pause for a bit; jamming from reverse to drive is a bad idea
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_3_S:
self._driver.drive(0.0, 0.0)
yield True
yield False
def _unstuck_yourself_iterator(self, seconds):
"""Commands the car to reverse and try to get off an obstacle."""
# The ESC requires us to send neutral throttle for a bit, then send
# reverse, then neutral, then reverse again (which will actually drive
# the car in reverse)
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_1_S:
self._driver.drive(0.0, 0.0)
yield True
start = time.time()
while time.time() < start + self.REVERSE_TIME_S:
self._driver.drive(-0.5, 0.0)
yield True
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_2_S:
self._driver.drive(0.0, 0.0)
yield True
telemetry = self._telemetry.get_data()
heading_d = telemetry['heading_d']
current_waypoint = self._waypoint_generator.get_current_waypoint(
telemetry['x_m'], telemetry['y_m'])
degrees = Telemetry.relative_degrees(telemetry['x_m'],
telemetry['y_m'],
current_waypoint[0],
current_waypoint[1])
is_left = Telemetry.is_turn_left(heading_d, degrees)
if is_left is None:
turn_direction = 1.0 if random.randint(0, 1) == 0 else -1.0
elif is_left:
turn_direction = 1.0 # Reversed because we're driving in reverse
else:
turn_direction = -1.0
start = time.time()
while time.time() < start + seconds:
self._driver.drive(-.5, turn_direction)
yield True
# Pause for a bit; jamming from reverse to drive is a bad idea
start = time.time()
while time.time() < start + self.NEUTRAL_TIME_3_S:
self._driver.drive(0.0, 0.0)
yield True
yield False
def _get_extended_waypoint(self):
"""Returns the extended waypoint."""
if self._current_waypoint_index == 0:
return self._waypoints[0]
if (self._current_waypoint_index >= 1
and self._current_waypoint_index < len(self._waypoints)):
previous_waypoint_m = self._waypoints[self._current_waypoint_index
- 1]
current_waypoint_m = self._waypoints[self._current_waypoint_index]
degrees = Telemetry.relative_degrees(previous_waypoint_m[0],
previous_waypoint_m[1],
current_waypoint_m[0],
current_waypoint_m[1])
offset_m = Telemetry.rotate_degrees_clockwise((0.0, self.BEYOND_M),
degrees)
return (current_waypoint_m[0] + offset_m[0],
current_waypoint_m[1] + offset_m[1])
return self._waypoints[-1]
def test_get_current_waypoint(self):
"""Tests the extension waypoint generation."""
points = ((20, 20),)
generator = ExtensionWaypointGenerator(points)
# If there's only one point, always return it
self.assertEqual(points[0], generator.get_current_waypoint(10, 10))
self.assertEqual(points[0], generator.get_current_waypoint(19, 19))
self.assertEqual(points[0], generator.get_current_waypoint(21, 19))
self.assertEqual(points[0], generator.get_current_waypoint(21, 21))
self.assertEqual(points[0], generator.get_current_waypoint(19, 21))
# Project through tests
position = (-100, 20)
points = ((0, 0), (0, 10), (10, 10), (10, 0), (0, 0), (5, 5), (5, 0), (5, -5), (0, 0))
generator = ExtensionWaypointGenerator(points)
self.assertEqual(
generator.get_current_waypoint(
position[0],
position[1]
),
points[0]
)
generator.next()
self.assertTrue(position not in points)
for index in range(1, len(points)):
point = points[index]
previous_point = points[index - 1]
true_waypoint = point
waypoint = generator.get_current_waypoint(
position[0],
position[1]
)
self.assertAlmostEqual(
Telemetry.relative_degrees(
previous_point[0],
previous_point[1],
true_waypoint[0],
true_waypoint[1]
),
Telemetry.relative_degrees(
previous_point[0],
previous_point[1],
waypoint[0],
waypoint[1]
)
)
def distance(x1, y1, x2, y2):
"""Returns distance between 2 points."""
return math.sqrt(
(x1 - x2) ** 2 + (y1 - y2) ** 2
)
self.assertLess(
distance(
previous_point[0],
previous_point[1],
true_waypoint[0],
true_waypoint[1]
) + 1.0,
distance(
previous_point[0],
previous_point[1],
waypoint[0],
waypoint[1]
)
)
generator.next()
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
