def modify(self, left, right): handle_time_stamp = self._handle_time_stamp if handle_time_stamp is None or time.time() - handle_time_stamp > 0.8: scan = None else: scan = self._scan _time_stamp = time.time() _delta_time = _time_stamp - self._time_stamp self._time_stamp = _time_stamp if scan is not None: current_angle = logic.get_angle(left, right, self._robo_width) current_speed = logic.get_speed(left, right) min_distance, min_distance_angle = logic.get_min_distance(scan, current_angle, self._scanner_dist_offset, self._angle_range) max_distance, max_distance_angle = logic.get_max_distance(scan, current_angle, self._scanner_dist_offset, self._angle_range) # print 'rodeo-swap : min: %d, %d; max: %d, %d' % (min_distance, math.degrees(min_distance_angle), # max_distance, math.degrees(max_distance_angle)) if min_distance is not None: soft_limit = logic.get_soft_limit(current_speed, self._max_speed, self._soft_limit, self._hard_limit, self._alpha) if min_distance < soft_limit: if max_distance_angle > current_angle or min_distance_angle < current_angle: if left > 0: max_speed = logic.get_max_speed(min_distance, soft_limit, self._hard_limit, self._max_rotating_speed) left = left if left < max_speed else max_speed right = -left # FIXME(paoolo) else: if right > 0: _t = left left = right right = _t elif max_distance_angle < current_angle or min_distance_angle > current_angle: if right > 0: max_speed = logic.get_max_speed(min_distance, soft_limit, self._hard_limit, self._max_rotating_speed) right = right if right < max_speed else max_speed left = -right # FIXME(paoolo) else: if left > 0: _t = right right = left left = _t elif min_distance < soft_limit * self._beta: left = -left right = -right else: print 'rodeo-swap: no scan!' left, right = 0.0, 0.0 return left, right