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
