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
    def modify(self, left, right):
        if abs(self._x - self._target_x) < self._target_radius \
                and abs(self._y - self._target_y) < self._target_radius:
            return 0.0, 0.0

        if self.roboclaw is None:
            return left, right

        _speed = self.roboclaw.get_current_motors_speed()
        _speed_left, speed_right = _speed.get_front_left_speed(), _speed.get_front_right_speed()

        if abs(_speed_left - left) > 500.0 or abs(speed_right - right) > 500.0:
            return left, right

        _time_stamp = time.time()
        _delta_time = _time_stamp - self._time_stamp
        self._time_stamp = _time_stamp

        self._x, self._y, self._robo_angle = self._compute_new_relative_position(_speed_left, speed_right, _delta_time)

        _target_angle = math.atan2(self._target_y - self._y, self._target_x - self._x)

        _drive_angle = _target_angle - self._robo_angle
        _drive_angle = DriveToPoint._normalize_angle(_drive_angle)

        if not self._active:
            self._active = (abs(_drive_angle) > 2.0 * math.pi / 3.0)
        else:
            self._active = not (abs(_drive_angle) < 0.03490658503988659)

        print 'drive_to_point: %s, %f, %f, %f, %f, %f, %f' % (str(self._active),
                                                              math.degrees(_target_angle),
                                                              math.degrees(self._robo_angle),
                                                              math.degrees(_drive_angle),
                                                              _drive_angle / math.pi,
                                                              self._x, self._y)

        if not self._active:
            return left, right

        else:
            _drive_speed = logic.get_speed(left, right)

            return _drive_speed - self._alpha * _drive_angle / math.pi * _drive_speed, \
                   _drive_speed + self._alpha * _drive_angle / math.pi * _drive_speed