Ejemplo n.º 1
0
Archivo: ro.py Proyecto: Vibri/heidi
def waypoints2dirDist( waypoints ):
  "convert GPS waypoints to desired heading and distance"
  conv = Convertor( waypoints[0] )
  ret = []
  for start,goal in zip(waypoints[:-1],waypoints[1:]):
    ret.append( (
        angleTo( conv.geo2planar(start), conv.geo2planar(goal) ),
        distance( conv.geo2planar(start), conv.geo2planar(goal) )
       ) )
  return ret
Ejemplo n.º 2
0
def waypoints2dirDist(waypoints):
    "convert GPS waypoints to desired heading and distance"
    conv = Convertor(waypoints[0])
    ret = []
    for start, goal in zip(waypoints[:-1], waypoints[1:]):
        ret.append((angleTo(conv.geo2planar(start), conv.geo2planar(goal)),
                    distance(conv.geo2planar(start), conv.geo2planar(goal))))
    return ret
Ejemplo n.º 3
0
def ver0(metalog, waypoints=None):
    assert metalog is not None
    assert waypoints is not None  # for simplicity (first is start)

    conv = Convertor(refPoint = waypoints[0]) 
    waypoints = waypoints[1:-1]  # remove start/finish

    can_log_name = metalog.getLog('can')
    if metalog.replay:
        if metalog.areAssertsEnabled():
            can = CAN(ReplayLog(can_log_name), skipInit=True)
        else:
            can = CAN(ReplayLogInputsOnly(can_log_name), skipInit=True)
    else:
        can = CAN()
        can.relog(can_log_name)
    can.resetModules(configFn=setup_faster_update)
    robot = JohnDeere(can=can)
    robot.UPDATE_TIME_FREQUENCY = 20.0  # TODO change internal and integrate setup

    robot.localization = None  # TODO


    # mount_sensor(GPS, robot, metalog)
    gps_log_name = metalog.getLog('gps')
    print gps_log_name
    if metalog.replay:
        robot.gps = DummySensor()
        function = SourceLogger(None, gps_log_name).get
    else:
        robot.gps = GPS(verbose=0)
        function = SourceLogger(robot.gps.coord, gps_log_name).get
    robot.gps_data = None
    robot.register_data_source('gps', function, gps_data_extension) 

    # mount_sensor(VelodyneThread, robot, metalog)
    velodyne_log_name = metalog.getLog('velodyne_dist')
    print velodyne_log_name
    sensor = Velodyne(metalog=metalog)
    if metalog.replay:
        robot.velodyne = DummySensor()
        function = SourceLogger(None, velodyne_log_name).get
    else:
        robot.velodyne = VelodyneThread(sensor)
        function = SourceLogger(robot.velodyne.scan_safe_dist, velodyne_log_name).get
    robot.velodyne_data = None
    robot.register_data_source('velodyne', function, velodyne_data_extension) 

    robot.gps.start()  # ASAP so we get GPS fix
    robot.velodyne.start()  # the data source is active, so it is necessary to read-out data

    center(robot)
    wait_for_start(robot)

    moving = False
    robot.desired_speed = 0.5
    start_time = robot.time
    prev_gps = robot.gps_data
    prev_destination_dist = None
    while robot.time - start_time < 30*60:  # RO timelimit 30 minutes
        robot.update()
        if robot.gps_data != prev_gps:
            print robot.time, robot.gas, robot.gps_data, robot.velodyne_data
            prev_gps = robot.gps_data
            if robot.gps_data is not None:
                dist_arr = [distance( conv.geo2planar((robot.gps_data[1], robot.gps_data[0])), 
                                      conv.geo2planar(destination) ) for destination in waypoints]
                dist = min(dist_arr)
                print "DIST-GPS", dist
                if prev_destination_dist is not None:
                    if prev_destination_dist < dist and dist < 10.0:
                        robot.drop_ball = True
                        # remove nearest
                        i = dist_arr.index(dist)  # ugly, but ...
                        print "INDEX", i
                        del waypoints[i]
                        center(robot)
                        moving = False
                        robot.wait(1.0)
                        robot.drop_ball = False
                        robot.wait(3.0)
                        dist = None
                prev_destination_dist = dist

        dist = None
        if robot.velodyne_data is not None:
            index, dist = robot.velodyne_data
            if dist is not None:
                dist = min(dist)  # currently supported tupple of readings
        if moving:
            if dist is None or dist < SAFE_DISTANCE_STOP:
                print "!!! STOP !!! -",  robot.velodyne_data
                center(robot)
                moving = False

            elif dist < TURN_DISTANCE:
                if abs(robot.steering_angle) < STRAIGHT_EPS:
                    arr = robot.velodyne_data[1]
                    num = len(arr)
                    left, right = min(arr[:num/2]), min(arr[num/2:])
                    print "DECIDE", left, right, robot.velodyne_data
                    if left <= right:
                        robot.pulse_right(RIGHT_TURN_TIME)
                        robot.steering_angle = math.radians(-30)  # TODO replace by autodetect
                    else:
                        robot.pulse_left(LEFT_TURN_TIME)
                        robot.steering_angle = math.radians(30)  # TODO replace by autodetect

            elif dist > NO_TURN_DISTANCE:
                if abs(robot.steering_angle) > STRAIGHT_EPS:
                    if robot.steering_angle < 0:
                        robot.pulse_left(LEFT_TURN_TIME)
                    else:
                        robot.pulse_right(RIGHT_TURN_TIME)
                    robot.steering_angle = 0.0  # TODO replace by autodetect

        else:  # not moving
            if dist is not None and dist > SAFE_DISTANCE_GO:
                print "GO",  robot.velodyne_data
                go(robot)
                moving = True
        if not robot.buttonGo:
            print "STOP!"
            break
    center(robot)
    robot.velodyne.requestStop()
    robot.gps.requestStop()