Ejemplo n.º 1
0
def demo(metalog):
    assert metalog is not None

    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

    # GPS
    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)

    # Velodyne
    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:  # limit 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
        dist = None
        if robot.velodyne_data is not None:
            dist_index = None
            if len(robot.velodyne_data) == 2:
                index, dist = robot.velodyne_data
            else:
                index, dist, dist_index = 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)
                robot.canproxy.stop()
                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.canproxy.set_turn_raw(-100)
                        robot.steering_angle = math.radians(
                            -30)  # TODO replace by autodetect
                    else:
                        robot.canproxy.set_turn_raw(100)
                        robot.steering_angle = math.radians(
                            30)  # TODO replace by autodetect

            elif dist > NO_TURN_DISTANCE:
                if abs(robot.steering_angle) > STRAIGHT_EPS:
                    robot.canproxy.set_turn_raw(0)
                    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)
                robot.canproxy.go()
                moving = True
        if not robot.buttonGo:
            print("STOP!")
            break
    robot.canproxy.stop_turn()
    center(robot)
    robot.velodyne.requestStop()
    robot.gps.requestStop()
Ejemplo n.º 2
0
def demo(metalog):
    assert metalog is not None

    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


    # GPS
    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) 

    # Laser
    laser_log_name = metalog.getLog('laser')
    print laser_log_name
    if metalog.replay:
        robot.laser = DummySensor()
        function = SourceLogger(None, laser_log_name).get
    else:
        robot.laser = LaserIP()
        function = SourceLogger(robot.laser.scan, laser_log_name).get
    robot.laser_data = None
    robot.register_data_source('laser', function, laser_data_extension) 

    robot.gps.start()  # ASAP so we get GPS fix
    robot.laser.start()

    while robot.gas is None:
        robot.update()

    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:  # limit 30 minutes
        robot.update()
        dist = None
        if robot.laser_data is not None:
            assert len(robot.laser_data) == 541,  len(robot.laser_data)
            distL, distR = min_dist_arr(robot.laser_data[200:-200])
            distL = 20.0 if distL is None else distL
            distR = 20.0 if distR is None else distR
            dist = min(distL, distR)
        if robot.gps_data != prev_gps:
            print robot.time, robot.gas, robot.gps_data, (distL, distR)
            prev_gps = robot.gps_data
        if moving:
            if dist is None or dist < SAFE_DISTANCE_STOP:
                print "!!! STOP !!!",  dist, (distL, distR)
                robot.canproxy.stop()
                moving = False

            elif dist < TURN_DISTANCE:
                if abs(robot.steering_angle) < STRAIGHT_EPS:
                    print "DECIDE", distL, distR
                    if distL <= distR:
                        robot.canproxy.set_turn_raw(-100)
                        robot.steering_angle = math.radians(-30)
                    else:
                        robot.canproxy.set_turn_raw(100)
                        robot.steering_angle = math.radians(30)

            elif dist > NO_TURN_DISTANCE:
                if abs(robot.steering_angle) > STRAIGHT_EPS:
                    robot.canproxy.set_turn_raw(0)
                    robot.steering_angle = 0.0

        else:  # not moving
            if dist is not None and dist > SAFE_DISTANCE_GO:
                print "GO",  dist
                robot.canproxy.go()
                moving = True
        if not robot.buttonGo:
            print "STOP!"
            break
    robot.canproxy.stop_turn()
    center(robot)
    robot.laser.requestStop()
    robot.gps.requestStop()
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()
Ejemplo n.º 4
0
def demo(metalog):
    assert metalog is not None

    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


    # GPS
    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) 

    # Velodyne
    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:  # limit 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
        dist = None
        if robot.velodyne_data is not None:
            dist_index = None
            if len(robot.velodyne_data) == 2:
                index, dist = robot.velodyne_data
            else:
                index, dist, dist_index = 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)
                robot.canproxy.stop()
                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.canproxy.set_turn_raw(-100)
                        robot.steering_angle = math.radians(-30)  # TODO replace by autodetect
                    else:
                        robot.canproxy.set_turn_raw(100)
                        robot.steering_angle = math.radians(30)  # TODO replace by autodetect

            elif dist > NO_TURN_DISTANCE:
                if abs(robot.steering_angle) > STRAIGHT_EPS:
                    robot.canproxy.set_turn_raw(0)
                    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)
                robot.canproxy.go()
                moving = True
        if not robot.buttonGo:
            print "STOP!"
            break
    robot.canproxy.stop_turn()
    center(robot)
    robot.velodyne.requestStop()
    robot.gps.requestStop()
Ejemplo n.º 5
0
def followme(metalog):
    assert metalog is not None

    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

    # GPS
    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)

    # Velodyne
    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
    target_detected = 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:  # limit 30 minutes
        robot.update()
        if robot.gps_data != prev_gps:
            if robot.velodyne_data is not None:
                print(robot.velodyne_data[-1], robot.canproxy.wheel_angle_raw,
                      robot.canproxy.desired_wheel_angle_raw)
            prev_gps = robot.gps_data

        dist, dist_index = None, None
        if robot.velodyne_data is not None:

            if len(robot.velodyne_data) == 2:
                index, dist = robot.velodyne_data
            else:
                index, dist, dist_index = robot.velodyne_data
            if dist is not None:
                dist = min(dist)  # currently supported tupple of readings

        if dist_index is not None:
            target_index, target_dist = dist_index
            robot.canproxy.set_turn_raw(int((-100 / 45.) * target_index))
            target_detected = target_dist < TARGET_DISTANCE
        else:
            dist_index = False

        if moving:
            if dist is None or dist < SAFE_DISTANCE_STOP or not target_detected:
                print "!!! STOP !!! -", robot.velodyne_data
                robot.canproxy.stop()
                moving = False

        else:  # not moving
            if dist is not None and dist > SAFE_DISTANCE_GO:
                print "GO", target_detected, robot.velodyne_data
                if target_detected:
                    robot.canproxy.go_slowly()
                    moving = True
        if not robot.buttonGo:
            print "STOP!"
            break
    robot.canproxy.stop_turn()
    center(robot)
    robot.velodyne.requestStop()
    robot.gps.requestStop()
Ejemplo n.º 6
0
def followme(metalog):
    assert metalog is not None

    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


    # GPS
    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) 

    # Velodyne
    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
    target_detected = 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:  # limit 30 minutes
        robot.update()
        if robot.gps_data != prev_gps:
            if robot.velodyne_data is not None:
                print((robot.velodyne_data[-1], robot.canproxy.wheel_angle_raw, 
                       robot.canproxy.desired_wheel_angle_raw))
            prev_gps = robot.gps_data

        dist, dist_index = None, None
        if robot.velodyne_data is not None:
            
            if len(robot.velodyne_data) == 2:
                index, dist = robot.velodyne_data
            else:
                index, dist, dist_index = robot.velodyne_data
            if dist is not None:
                dist = min(dist)  # currently supported tupple of readings

        if dist_index is not None:
            target_index, target_dist = dist_index
            robot.canproxy.set_turn_raw(int((-100/45.)*target_index))
            target_detected = target_dist < TARGET_DISTANCE
        else:
            dist_index = False

        if moving:
            if dist is None or dist < SAFE_DISTANCE_STOP or not target_detected:
                print("!!! STOP !!! -",  robot.velodyne_data)
                robot.canproxy.stop()
                moving = False

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