def auto_update_lidar_cloud(self, only_nearby_meters=4, fps=2, cb=None):
ts = None
while True:
ts = keep_rps(ts, fps=fps)
points = self._get_lidar_points()
if len(points) == 0:
break
if cb is not None:
cb(points, only_nearby_meters=only_nearby_meters)
def _capture_frames(self):
self.cap = cv2.VideoCapture(self._get_csi_gsreamer_str(),
cv2.CAP_GSTREAMER)
ts = None
while True:
ts = keep_rps(ts, fps=self.fps)
ret, frame = self.cap.read()
if not ret:
break
self.value = frame
def start(self):
previous = None
ts = None
while True:
ts = keep_rps(ts, fps=self.fps)
current = self._get_current()
if previous is None:
self.poses.append({
'x': current['odometry_dx'],
'y': current['odometry_dy'],
'teta': current['teta'],
})
else:
dt = current['ts'] - previous['ts']
if dt == 0.:
continue
try:
current['odometry_dx'], current[
'odometry_dy'] = self.odometry.get_dx_dy(
dt, current['teta'])
except Exception as e:
self.errors.append('odometry: {}'.format(e))
raise
try:
dx, dy, teta = self._get_transformation(
dt, current, previous)
except Exception as e:
print('_get_transformation failed with {}'.format(e),
file=sys.stderr)
raise
prev_pose = self.poses[-1]
self.poses.append({
'ts': current['ts'],
'x': prev_pose['x'] + dx,
'y': prev_pose['y'] + dy,
'teta': teta,
})
self._upgrade_current(current)
previous = current
def run(self, save_track_info=False):
def tracker_run():
self.tracker.start()
def detector_run():
self.detector.start()
if not self.test_run:
run_as_thread(tracker_run)
run_as_thread(detector_run)
if self.distance_sensors is not None:
self.distance_sensors.start()
ts = None
direction = {'steering': 0., 'throttle': self.STOP}
steps_with_direction = 0
keep_for = 0
while True:
ts = keep_rps(ts, fps=4)
prev_direction = direction
if keep_for <= 0:
direction, keep_for = self._get_next_move(direction, steps_with_direction)
keep_for -= 1
# print('direction: {}, turn: {}, speed {:0.4f}'.format(
# direction['throttle'], direction['steering'], self.odometry.get_speed()
# ))
self._follow_direction(direction)
if save_track_info:
self.track_info.append({
'speed': self.cached_speed,
'pose': self.cached_pose,
'points': self.cached_points,
'direction': self.cached_direction,
'weights': self.cached_weights,
})
if prev_direction == direction:
steps_with_direction += 1
else:
steps_with_direction = 0
def update_odometry_cycle(self):
import RPi.GPIO as GPIO
GPIO.setmode(GPIO.BCM) # BCM pin-numbering scheme from Raspberry Pi
GPIO.setup(ODOMETRY_PRIMARY_PIN, GPIO.IN)
GPIO.setup(ODOMETRY_SECONDARY_PIN, GPIO.IN)
detections = self.event_timestamps
def primary_both(_):
if GPIO.input(ODOMETRY_PRIMARY_PIN) == GPIO.HIGH:
detections['primary_rising'] = time()
detections['any_event'] = detections['primary_rising']
else:
ts = time()
if detections['primary_falling'] is not None:
prev_ts = detections['primary_falling']
an_interval = ts - prev_ts
if 0.05 < an_interval < 2.:
self.odometry_last_interval = an_interval
detections['primary_falling'] = ts
detections['any_event'] = detections['primary_falling']
def secondary_both(_):
if GPIO.input(ODOMETRY_SECONDARY_PIN) == GPIO.HIGH:
detections['secondary_rising'] = time()
detections['any_event'] = detections['secondary_rising']
self.odometry_counter += 1
else:
detections['secondary_falling'] = time()
detections['any_event'] = detections['secondary_falling']
try:
GPIO.add_event_detect(ODOMETRY_PRIMARY_PIN,
GPIO.BOTH,
callback=primary_both)
GPIO.add_event_detect(ODOMETRY_SECONDARY_PIN,
GPIO.BOTH,
callback=secondary_both)
ts = None
while True:
ts = keep_rps(ts, fps=2)
if detections['any_event'] is None or time(
) - detections['any_event'] > 0.5:
# reset state if no events (car stopped)
self.odometry_last_interval = None
self.direction = 0.
for k in detections.keys():
detections[k] = None
elif detections['primary_falling'] is not None and detections['secondary_rising'] is not None \
and (detections['primary_rising'] is not None or detections['secondary_falling'] is not None):
# select events to determine direction
select_rising = False
if detections['primary_rising'] is not None and detections[
'secondary_falling'] is not None:
if abs(detections['primary_rising'] - detections['secondary_rising']) < \
abs(detections['primary_falling'] - detections['secondary_falling']):
select_rising = True
else:
select_rising = False
# determine direction
if select_rising and detections[
'primary_rising'] is not None:
self.direction = 1. if detections[
'primary_rising'] < detections[
'secondary_rising'] else -1.
else:
self.direction = 1. if detections[
'primary_falling'] < detections[
'secondary_falling'] else -1.
finally:
GPIO.remove_event_detect(ODOMETRY_PRIMARY_PIN)
GPIO.remove_event_detect(ODOMETRY_SECONDARY_PIN)
GPIO.cleanup()