def on_update(self, hist):
pipeline_result = hist[-1]
logging.debug("WaitState Pipeline results {}".format(hist[-1]))
# unpack results
cam_ok, bt_ok, us_ok, ir_ok = self.pipeline[
"y_deviation"].success_state, \
self.pipeline[
"user_distance"].success_state, \
self.pipeline["us_change"].success_state, \
self.pipeline["ir_change"].success_state
dev, distance, _, _ = pipeline_result
# if there are no result values go to wait state
if not cam_ok and not bt_ok:
if (us_ok or ir_ok) and current_time_millis(
) - self.start_time > config.IF_US_START_DELAY:
return SearchState(self.state_machine)
return self
if not cam_ok and bt_ok:
# is bt distance far then go in wait state or timeout is reached go
# in wait state
if distance == bluetooth.UserDistanceEstimationPipeline.Distance.NEAR \
or ((us_ok or ir_ok) and current_time_millis() - self.start_time > config.IF_US_START_DELAY):
return SearchState(self.state_machine)
else:
return self
if cam_ok and not bt_ok:
return TrackState(self.state_machine)
if cam_ok and bt_ok:
return FollowState(self.state_machine)
def check_us_sensor_data_changed(
self,
time_threshold=config.US_TIME_THRESHOLD,
distance_threshold=config.US_DISTANCE_THRESHOLD):
""" Return true if data changed by more than distance threshold in time_threshold"""
upper_threshold = current_time_millis() - time_threshold
under_threshold = current_time_millis() - time_threshold * 2
upper_avg = []
under_avg = []
try:
self.lock.acquire()
if len(self.data_deque) == 0:
return None
for i in reversed(self.data_deque):
if i.time > upper_threshold:
upper_avg.append(i.distance)
if under_threshold < i.time < upper_threshold:
under_avg.append(i.distance)
finally:
self.lock.release()
if len(upper_avg) == 0 or len(under_avg) == 0:
return False
upper_avg = sum(upper_avg) / len(upper_avg)
under_avg = sum(under_avg) / len(under_avg)
if abs(upper_avg - under_avg) > distance_threshold:
return True
return False
def queue_next_state(self, next_state):
if self.next_state != type(next_state):
self.next_state = type(next_state)
self.state_switching_timestamp = current_time_millis()
if current_time_millis(
) - self.state_switching_timestamp > config.STATE_SWITCH_COOLDOWN:
return next_state
else:
return self
def update(self):
logging.debug("\nUpdating state machine (current state: " +
str(self._current_state) + ")")
start = current_time_millis()
pipeline_out = self._current_state.pipeline.run_pipeline(None)
self.__history.append(pipeline_out)
next_state = self._current_state.on_update(self.__history)
self.set_state(next_state)
logging.debug("State machine updated ({}ms). New state: {}".format(
current_time_millis() - start, self._current_state))
def accumulate_distance(self):
"""
returns the distance in cm max value means object ist too far or too close
:return:
"""
while True:
# setze Trigger auf HIGH
GPIO.output(config.US_GPIO_TRIGGER, True)
# setze Trigger nach 0.01ms aus LOW
time.sleep(0.01)
GPIO.output(config.US_GPIO_TRIGGER, False)
StartZeit = time.time()
StopZeit = time.time()
timeout = current_time_millis()
# speichere Startzeit
while GPIO.input(config.US_GPIO_ECHO
) == 0 and current_time_millis() - timeout < 100:
StartZeit = time.time()
timeout = current_time_millis()
# speichere Ankunftszeit
while GPIO.input(config.US_GPIO_ECHO
) == 1 and current_time_millis() - timeout < 100:
StopZeit = time.time()
# Zeit Differenz zwischen Start und Ankunft
TimeElapsed = StopZeit - StartZeit
# mit der Schallgeschwindigkeit (34300 cm/s) multiplizieren
# und durch 2 teilen, da hin und zurueck
distance = (TimeElapsed * 34300) / 2
try:
self.lock.acquire()
self.data_deque.append(
DataTuple(current_time_millis(),
max(0.0, min(distance, config.US_MAX_VALUE))))
finally:
self.lock.release()
self.remove_old_data()
def on_enter(self):
self.state_machine.gesture_control.change_gesture("search")
if self.start_spin_direction == "left":
self.state_machine.robots_control.left(config.SEARCH_SPEED)
else:
self.state_machine.robots_control.right(config.SEARCH_SPEED)
self.start_time = current_time_millis()
def run_pipeline(self, inp):
self.reset_pipeline()
start = current_time_millis()
succ, out = self._execute(inp)
exectime = current_time_millis() - start
start = current_time_millis()
for cb in self.execute_callbacks:
cb(inp, out)
callbacktime = current_time_millis() - start
logging.debug(self.debug_prefix + "Executing pipeline {} took {}ms (callbacktime: {}ms)".format(
self.__class__.__name__, exectime, callbacktime))
self.__succ = succ
self.__output = out
return out
def remove_old_data(self, threshold=10000):
"""Removes data tuples from the queue that are older
than threshold milliseconds"""
threshold = current_time_millis() - threshold
try:
self.lock.acquire()
while (len(self.data) > 0 and self.data[0].time < threshold):
self.data.popleft()
finally:
self.lock.release()
def remove_old_data(self, threshold=config.US_DATA_ACC_THRESHOLD):
"""Removes data tuples from the queue that are older
than threshold milliseconds"""
threshold = current_time_millis() - threshold
try:
self.lock.acquire()
while len(self.data_deque
) > 0 and self.data_deque[0].time < threshold:
self.data_deque.popleft()
finally:
self.lock.release()
def on_update(self, hist):
pipeline_result = hist[-1]
logging.debug("SearchState Pipeline results {}".format(hist[-1]))
# unpack results
cam_ok, bt_ok = self.pipeline["y_deviation"].success_state, \
self.pipeline["user_distance"].success_state
dev, distance = pipeline_result
# if there are no result values go to wait state
if not cam_ok and not bt_ok:
if current_time_millis() - self.start_time > config.SEARCH_TIMEOUT:
return WaitState(self.state_machine)
return self
if not cam_ok and bt_ok:
# is bt distance far then go in wait state or timeout is reached go
# in wait state
if current_time_millis() - self.start_time > config.SEARCH_TIMEOUT or \
distance == bluetooth.UserDistanceEstimationPipeline.Distance.FAR:
return WaitState(self.state_machine)
return self
if cam_ok and not bt_ok:
return TrackState(self.state_machine)
if cam_ok and bt_ok:
return FollowState(self.state_machine)
def snapshot_data(self):
"""Returns a snapshot of the data in form of a DataList object.
This contains all data that has been collected in the last threshold
milliseconds"""
threshold = current_time_millis() - config.BT_TIME_THRESHOLD
data_list = []
try:
self.lock.acquire()
for t in reversed(self.data):
# Stop when data is too old
if t.time < threshold:
break
data_list.append(t)
finally:
self.lock.release()
return DataList(threshold, data_list)
def add_data(self, rssi):
"""Adds a new rssi value. This also calls self.remove_old_data()"""
# Positive rssi values are very rare, and indicate a very
# good connection. We simplify this by setting the value to
# 0, which indicates the best possible strength in our terms.
if rssi > 0:
rssi = 0
# Remove old entries from the queue that are older than 10 sec
self.remove_old_data()
# Add the new rssi value to the data queue
try:
self.lock.acquire()
self.data.append(
DataTuple(current_time_millis(),
abs(rssi) + self.offset))
finally:
self.lock.release()
def accumulate_distance(self):
"""
returns the distance in cm max value means object ist too far or too close
:return:
"""
while True:
try:
value = self.BP.get_sensor(self.PORT)
except brickpi3.SensorError as error:
value = None
if value:
try:
self.lock.acquire()
self.data_deque.append(
DataTuple(current_time_millis(), value))
finally:
self.lock.release()
self.remove_old_data()
def _execute(self, inp):
height, width = inp.shape
startt = current_time_millis()
horizontal_segments = []
for y in range(0, height, SCANLINE_DISTANCE):
start = None
for x in range(0, width):
if self.__pixel_in_range(inp[y, x]):
if start is None:
start = x
elif start is not None and x - start > SCANLINE_DISTANCE:
horizontal_segments.append((y, start, x))
start = None
print("HSCAN", current_time_millis() - startt)
startt = current_time_millis()
vertical_segments = []
for x in range(0, width, SCANLINE_DISTANCE):
start = None
for y in range(0, height):
if self.__pixel_in_range(inp[y, x]):
if start is None:
start = y
elif start is not None and y - start > SCANLINE_DISTANCE:
vertical_segments.append((x, start, y))
start = None
print("VSCAN", current_time_millis() - startt)
startt = current_time_millis()
largest_bbox = None
largest_area = 0
for (hy, hx1, hx2), (vx, vy1,
vy2) in itertools.product(horizontal_segments,
vertical_segments):
if vy1 <= hy <= vy2 and hx1 <= vx <= hx2:
w, h = hx2 - hx1, vy2 - vy1
if w * h > largest_area:
largest_bbox = (hx1, vy1, w, h)
print("BBOX", current_time_millis() - startt)
return largest_bbox is None, largest_bbox
def on_enter(self):
self.state_machine.gesture_control.change_gesture("wait")
self.start_time = current_time_millis()
