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()