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 test_is_turn_left(self):
self.assertTrue(Telemetry.is_turn_left(1, 0))
self.assertFalse(Telemetry.is_turn_left(0, 1))
self.assertTrue(Telemetry.is_turn_left(1, 359))
self.assertFalse(Telemetry.is_turn_left(359, 1))
self.assertTrue(Telemetry.is_turn_left(1, 270))
self.assertTrue(Telemetry.is_turn_left(0, 270))
self.assertTrue(Telemetry.is_turn_left(359, 270))
self.assertFalse(Telemetry.is_turn_left(1, 90))
self.assertFalse(Telemetry.is_turn_left(0, 90))
self.assertFalse(Telemetry.is_turn_left(359, 90))
# These shouldn't throw
Telemetry.is_turn_left(0, 0)
Telemetry.is_turn_left(1, 1)
Telemetry.is_turn_left(180, 180)
Telemetry.is_turn_left(180, 0)
Telemetry.is_turn_left(270, 90)
Telemetry.is_turn_left(360, 0)
Telemetry.is_turn_left(0, 360)
def test_is_turn_left(self):
self.assertTrue(Telemetry.is_turn_left(1, 0))
self.assertFalse(Telemetry.is_turn_left(0, 1))
self.assertTrue(Telemetry.is_turn_left(1, 359))
self.assertFalse(Telemetry.is_turn_left(359, 1))
self.assertTrue(Telemetry.is_turn_left(1, 270))
self.assertTrue(Telemetry.is_turn_left(0, 270))
self.assertTrue(Telemetry.is_turn_left(359, 270))
self.assertFalse(Telemetry.is_turn_left(1, 90))
self.assertFalse(Telemetry.is_turn_left(0, 90))
self.assertFalse(Telemetry.is_turn_left(359, 90))
# These shouldn't throw
Telemetry.is_turn_left(0, 0)
Telemetry.is_turn_left(1, 1)
Telemetry.is_turn_left(180, 180)
Telemetry.is_turn_left(180, 0)
Telemetry.is_turn_left(270, 90)
Telemetry.is_turn_left(360, 0)
Telemetry.is_turn_left(0, 360)
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