Exemplo n.º 1
0
def navigate_pattern(metalog, conf, viewer=None):
    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,
                  timestamps_log=open(metalog.getLog('timestamps'), 'w'))
    can.resetModules(configFn=setup_faster_update)

    if conf is not None and 'localization' in conf.data:
        loc = SimpleOdometry.from_dict(conf.data['localization'])
    else:
        loc = SimpleOdometry(pose=(0.0, 2.5, 0.0))
        loc.global_map = [(0.0, 0.0), (15.0, 0.0), (15.0, 5.0), (0.0, 5.0)]

    jd_config = None
    if conf is not None and 'johndeere' in conf.data:
        jd_config = conf.data['johndeere']
    robot = JohnDeere(can=can, localization=loc, config=jd_config)
    robot.UPDATE_TIME_FREQUENCY = 20.0  # TODO change internal and integrate setup

    for sensor_name in ['gps', 'laser', 'camera']:
        attach_sensor(robot, sensor_name, metalog)
    attach_processor(robot, metalog, image_callback)

    long_side = max([x for x, y in robot.localization.global_map])

    robot.canproxy.stop()
    robot.canproxy.set_turn_raw(0)

    if viewer is not None:
        robot.extensions.append(('viewer', viewer_extension))

    speed = 0.5

    try:
        robot.extensions.append(('detect_near', detect_near_extension))
        robot.extensions.append(('emergency_stop', emergency_stop_extension))

        for i in xrange(10):
            #            run_oval(robot, speed)
            run_there_and_back(robot, long_side, speed)

    except NearObstacle:
        print "Near Exception Raised!"
        robot.extensions = []  # hack
    except EmergencyStopException:
        print "Emergency STOP Exception!"
        robot.extensions = []  # hack

    robot.canproxy.stop()
    robot.canproxy.stop_turn()
    robot.wait(3.0)

    detach_all_sensors(robot)
Exemplo n.º 2
0
def driver_self_test(driver, 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

    robot.canproxy.stop()
    robot.canproxy.set_turn_raw(0)

    go_straight(robot, distance=1.0, speed=0.3, with_stop=True)

    for i in range(3):
        turn(robot, math.radians(-45), radius=2.6, speed=0.5)
        turn(robot, math.radians(-45), radius=2.6, speed=-0.5)

    robot.canproxy.stop_turn()
    robot.wait(3.0)
Exemplo n.º 3
0
def driver_self_test(driver, 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

    robot.canproxy.stop()
    robot.canproxy.set_turn_raw(0)

    go_straight(robot, distance=1.0, speed=0.3, with_stop=True)

    for i in xrange(3):
        turn(robot, math.radians(-45), radius=2.6, speed=0.5)
        turn(robot, math.radians(-45), radius=2.6, speed=-0.5)

    robot.canproxy.stop_turn()
    robot.wait(3.0)
Exemplo n.º 4
0
def self_test(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.canproxy.stop()  # center(robot)
    wait_for_start(robot)
    robot.desired_speed = 0.5
    start_time = robot.time
    robot.canproxy.set_turn_raw(0)
    robot.canproxy.go()  # go(robot)
    while robot.time - start_time < 333.0:
        robot.update()
        print robot.time, robot.gas
        if not robot.buttonGo:
            print "STOP!"
            break
    robot.canproxy.stop_turn()
    center(robot)  # keep old version for the first test
Exemplo n.º 5
0
def create_robot(metalog, conf):
    assert metalog is not None
    assert conf is not None  # config is required!

    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, timestamps_log=open(metalog.getLog('timestamps'), 'w'))
    can.resetModules(configFn=setup_faster_update)

    loc = SimpleOdometry.from_dict(conf.data.get('localization'))
    robot = JohnDeere(can=can, localization=loc, config=conf.data.get('johndeere'))
    robot.UPDATE_TIME_FREQUENCY = 20.0  # TODO change internal and integrate setup

    return robot
Exemplo n.º 6
0
def create_robot(metalog, conf):
    assert metalog is not None
    assert conf is not None  # config is required!

    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,
                  timestamps_log=open(metalog.getLog('timestamps'), 'w'))
    can.resetModules(configFn=setup_faster_update)

    loc = SimpleOdometry.from_dict(conf.data.get('localization'))
    robot = JohnDeere(can=can,
                      localization=loc,
                      config=conf.data.get('johndeere'))
    robot.UPDATE_TIME_FREQUENCY = 20.0  # TODO change internal and integrate setup

    return robot
Exemplo n.º 7
0
def self_test(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(speed=CAN.CAN_SPEED_1MB)
        can.relog(can_log_name)
    #can.resetModules()
    robot = Spider3(can=can)
    robot.localization = None

    try:
        robot.extensions.append(('emergency_stop', emergency_stop_extension))
        run_it(robot)
    except EmergencyStopException:
        print("Emergency STOP Exception!")

    robot.extensions = []
    robot.stop()
    robot.wait(3.0)
Exemplo n.º 8
0
def self_test(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(speed=CAN.CAN_SPEED_1MB)
        can.relog(can_log_name)
    #can.resetModules()
    robot = Spider3(can=can)
    robot.localization = None

    try:
        robot.extensions.append(('emergency_stop', emergency_stop_extension))
        run_it(robot)
    except EmergencyStopException:
        print("Emergency STOP Exception!")

    robot.extensions = []
    robot.stop()
    robot.wait(3.0)
Exemplo n.º 9
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()
Exemplo n.º 10
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()
Exemplo n.º 11
0
def drive_remotely(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

    soc = metalog.createLoggedSocket('remote', headerFormat=None)  # TODO fix headerFormat
    soc.bind( ('',PORT) )
    if not metalog.replay:
        soc.soc.setblocking(0)
        soc.soc.settimeout( SOCKET_TIMEOUT )
    
    remote_cmd_log_name = metalog.getLog('remote_cmd')
    if metalog.replay:
        robot.remote = DummySensor()
        function = SourceLogger(None, remote_cmd_log_name).get
    else:
        robot.remote = RemoteThread(soc)
        function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get

    max_speed = None

    robot.remote_data = None
    robot.register_data_source('remote', function, remote_data_extension) 
    robot.remote.start()

    robot.canproxy.stop()
    robot.canproxy.set_turn_raw(0)

    print("Waiting for remote commands ...")
    while robot.remote_data is None:
        robot.update()
    print("received", robot.remote_data.strip())

    moving = False
    turning = False
    while robot.remote_data != 'END\n':
        robot.update()
        if robot.remote_data == 'STOP\n' and (moving or turning):
            if moving:
                robot.canproxy.stop()
                print("STOP")
                moving = False
            if turning:
                robot.canproxy.stop_turn()
                print("STOP turn")
                turning = False
        elif robot.remote_data == 'UP\n' and not moving:
            if max_speed is None:
                robot.canproxy.go()
                print("GO")
            else:
                robot.set_desired_speed(max_speed)
                print("GO", max_speed)
            moving = True
        elif robot.remote_data == 'DOWN\n' and not moving:
            if max_speed is None:
                robot.canproxy.go_back()
                print("GO back")
            else:
                robot.set_desired_speed(-max_speed)
                print("GO back", max_speed)
            moving = True
        elif robot.remote_data == 'LEFT\n':
            robot.canproxy.set_turn_raw(200)
            print("Left")
            turning = True
        elif robot.remote_data == 'RIGHT\n':
            robot.canproxy.set_turn_raw(-200)
            print("Right")
            turning = True
        elif robot.remote_data == 'SPEED0\n':
            max_speed = None
        elif robot.remote_data.startswith('SPEED'):
            max_speed = int(robot.remote_data[len('SPEED'):])/10.0
            print("Max Speed", max_speed)

    print("received", robot.remote_data.strip())

    robot.canproxy.stop_turn()
    robot.remote.requestStop()
    robot.wait(3.0)
Exemplo n.º 12
0
def launch(cmd_args, robotFactory, task, configFn = None, canArgs={}):
  '''
  Launches a robot to the given file.

  Parameters:
  cmd_args     ... List of parameters in the "configFile [--file logfile [F|FF]]" form.
  robotFactory ... Function (or class) which can be called with the following named
                   parameters and creates an instance of the robot:
		   robot = robotFactory(can=..., replyLog=...)
  task         ... Factory which called as "t = task(robot, configFileName, verbose=...)"
                   creates a runable instance. Ie. "t()" runs the show. 
  configFn     ... A method called in the robot's preconfigration mode.
  canArgs      ... Named arguments passed down to the CAN.
  '''
  if len(cmd_args) < 2:
    print __doc__
    sys.exit(-1)
  configFilename = cmd_args[1]
  logName = None
  simulation = None
  metaLog = None

  if len(cmd_args) > 2:
    if cmd_args[2]=='--file':
      logName = cmd_args[3]
      if "meta" in logName:
        runNumber = 1
        try:
          if 'F' == cmd_args[-1] or 'FF' == cmd_args[-1]:
            runNumber = int(cmd_args[-2])
          else:
            runNumber = int(cmd_args[-1])
        except:
          pass
        print "METALOG", logName, runNumber
        metaLog = open(logName)
        for line in metaLog:
          if line.startswith("CANlog:"):
            runNumber -= 1
            if runNumber <= 0:
              logName = logName[:logName.find("meta")]+line.split()[1].split("/")[-1]
              break
      if len(cmd_args)> 4 and cmd_args[-1]=='FF':
        can = CAN( ReplyLogInputsOnly( logName ), skipInit = True )
      else:
        assertWrite = not (len(cmd_args) > 4 and cmd_args[-1]=='F') 
        can = CAN( ReplyLog( logName, assertWrite=assertWrite ), skipInit = True ) 
    else:
      assert( cmd_args[2]=='--simul' )
      simulation = True # TODO
  else:
    metaLog = open( timeName("logs/meta", "log"), "w" )
    metaLog.write( str(cmd_args) + "\n" )
    metaLog.write( "configFilename: '" + str(configFilename) + "'\n" )
    if configFilename:
      try:
        metaLog.write( "------------------\n" )
        metaLog.write( open(configFilename).read() )
        metaLog.write( "------------------\n" )
      except:
        metaLog.write( "Load FAILED\n" )
    metaLog.flush()

    can = CAN(**canArgs)

  isFromLog = logName is not None

  while 1:
    if not isFromLog and not simulation:
      can.relog('logs/s')
      print starter.starter( can, configFn=configFn )
      metaLog.write( "CANlog:\t" + str(can.relog('logs/r')) + "\n" )
      metaLog.flush()

    if simulation:
      robot = Simulator()
    else:
      robot = robotFactory(can=can, replyLog=logName, metaLog=metaLog)

    if isFromLog or simulation:
      viewlog.viewLogFile = open( "view.log", "w" )
      robot.addExtension( viewLogExtension )
#      robot.addExtension( viewCompassExtension )
      robot.addExtension( viewPoseExtension )
      laserViewer = LaserViewer( robot, distanceLimit = 2.0, sideLimit = 10 )
    if isFromLog:
      view = ViewCameraExtension( os.path.dirname( sys.argv[3] ) )
      robot.addExtension( view.viewCameraExtension )         
  
    if not __run( robot, task, configFilename, verbose=(isFromLog or simulation) ) or isFromLog or simulation:
      break
    del robot # robot has switch to preoperation ... probably not a good idea
Exemplo n.º 13
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()
Exemplo n.º 14
0
Arquivo: rr.py Projeto: tajgr/osgar
def robot_go_straight(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, timestamps_log=open(metalog.getLog('timestamps'), 'w'))
    can.resetModules(configFn=setup_faster_update)
    robot = JohnDeere(can=can, localization=SimpleOdometry())
    robot.UPDATE_TIME_FREQUENCY = 20.0  # TODO change internal and integrate setup

    for sensor_name in ['gps', 'laser', 'camera']:
        attach_sensor(robot, sensor_name, metalog)

    robot.canproxy.stop()
    robot.set_desired_steering(0.0)  # i.e. go straight (!)

    try:
        robot.extensions.append(('emergency_stop', emergency_stop_extension))
        print(robot.canproxy.buttons_and_LEDs)
        wait_for_start(robot)
        print(robot.canproxy.buttons_and_LEDs)

        prev_laser = None
        last_laser_update = None
        moving = False
        dist = None
        distL, distR = None, None

        prev_camera = None
        global_offset = 0.0

        while True:
            robot.update()
            if robot.laser_data is not None:
                assert len(robot.laser_data) == 541, len(robot.laser_data)
                if robot.laser_data != prev_laser:
                    prev_laser = robot.laser_data
                    last_laser_update = robot.time
                    distL = min_dist(robot.laser_data[:180], INFINITY)
                    dist = min_dist(robot.laser_data[180:360], INFINITY)
                    distR = min_dist(robot.laser_data[360:], INFINITY)

            print("dist", distL, dist, distR)

            if prev_camera != robot.camera_data:
                print("CAMERA", robot.camera_data)
                prev_camera = robot.camera_data

#                filename = 'm:\\git\\osgar\\logs\\pisek170513\\game3\\' + os.path.basename(prev_camera[0])
                filename = prev_camera[0]
                img = cv2.imread(filename)
                if img is not None:
                    img = img[2*768/3:,:,:]
                    r = img[:,:,0]
                    g = img[:,:,1]
                    b = img[:,:,2]
                    mask = np.logical_and(g > b, g > r)
                    img[mask] = 0
                    left = mask[:, :512].sum()
                    right = mask[:, 512:].sum()
#                    print "LEFT_RIGHT", filename, left, right
                    if left > GREEN_LIMIT or right > GREEN_LIMIT:
                        if left < right:
                            global_offset = math.radians(2.5)
                        else:
                            global_offset = math.radians(-2.5)
                    else:
                        global_offset = 0.0
                robot.set_desired_steering(0.0 + global_offset)

            if moving:
                if dist is None or dist < SAFE_DISTANCE_STOP or min(distL, distR) < SAFE_SIDE_DISTANCE_STOP:
                    print("!!! STOP !!!",  distL, dist, distR)
                    robot.canproxy.stop()
                    moving = False
            else:  # not moving
                if dist is not None and dist > SAFE_DISTANCE_GO and min(distL, distR) > SAFE_SIDE_DISTANCE_GO:
                    print("GO",  distL, dist, distR)
                    robot.set_desired_speed(DESIRED_SPEED)
                    moving = True                

            if last_laser_update is not None and robot.time - last_laser_update > 0.3:
                print("!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update))
                dist = None  # no longer valid distance measurements
            
    except EmergencyStopException:
        print("Emergency STOP Exception!")
        robot.extensions = []  # hack

    robot.canproxy.stop()
    robot.canproxy.stop_turn()
    robot.wait(3.0)
    
    detach_all_sensors(robot)
Exemplo n.º 15
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()
Exemplo n.º 16
0
def self_test(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.canproxy.stop()
#    wait_for_start(robot)
    robot.desired_speed = 0.5
    start_time = robot.time
    robot.canproxy.set_turn_raw(0)
    robot.canproxy.go()
    start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
    arr = []
    while robot.time - start_time < 10.0:
        robot.update()
        arr.append(robot.canproxy.gas)
        dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                          - start_dist)/2.0
        if dist > 1.0:
            print "Dist OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
            break
    print dist
    robot.stop()
    robot.wait(3.0)
    dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                      - start_dist)/2.0
    print dist
    print

    robot.canproxy.go_back()
    start_time = robot.time
    start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
    arr = []
    while robot.time - start_time < 10.0:
        robot.update()
        arr.append(robot.canproxy.gas)
        dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                          - start_dist)/2.0
        if dist < -1.0:
            print "Dist back OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
            break
    print dist


#        print robot.time, robot.canproxy.gas
#        if not robot.buttonGo:
#            print "STOP!"
#            break
    robot.canproxy.stop_turn()
    robot.stop()
    robot.wait(3.0)
    dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                      - start_dist)/2.0
    print dist
Exemplo n.º 17
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()
Exemplo n.º 18
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()

    print "Wait for gas"
    while robot.canproxy.gas is None:
        robot.update()

    print "Wait for center"
    robot.canproxy.stop()
    # not available now :( ... wait_for_start(robot)

    moving = False
    robot.desired_speed = 0.5
    start_time = robot.time
    prev_gps = robot.gps_data
    prev_destination_dist = None
    last_laser_update = None
    prev_laser = None
    while robot.time - start_time < 3*60:  # 3min
        robot.update()
        dist = None
        turn_angle = None
        if robot.laser_data is not None:
            assert len(robot.laser_data) == 541, len(robot.laser_data)
            if robot.laser_data != prev_laser:
                prev_laser = robot.laser_data
                last_laser_update = robot.time
            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)
            turn_angle = follow_wall_angle(robot.laser_data, radius=1.5)

        if last_laser_update is not None and robot.time - last_laser_update > 0.3:
            print "!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update)
            dist = None  # no longer valid distance measurements

        if robot.gps_data != prev_gps:
            if turn_angle is not None:
                print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), math.degrees(turn_angle)
            else:
                print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), turn_angle
            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
        else:  # not moving
            if dist is not None and dist > SAFE_DISTANCE_GO:
                print "GO",  dist
                robot.canproxy.go()
                moving = True
        if turn_angle is not None:
            turn_cmd = max(-120, min(120, 2*math.degrees(turn_angle)))
            robot.canproxy.set_turn_raw(turn_cmd)
#            print turn_cmd
#        if not robot.buttonGo:
#            print "STOP!"
#            break
    robot.canproxy.stop_turn()
    robot.canproxy.stop()
    for i in xrange(20):
        robot.update()
    robot.laser.requestStop()
    robot.gps.requestStop()
Exemplo n.º 19
0
def launch(cmd_args, robotFactory, task, configFn=None, canArgs={}):
    '''
  Launches a robot to the given file.

  Parameters:
  cmd_args     ... List of parameters in the "configFile [--file logfile [F|FF]]" form.
  robotFactory ... Function (or class) which can be called with the following named
                   parameters and creates an instance of the robot:
		   robot = robotFactory(can=..., replyLog=...)
  task         ... Factory which called as "t = task(robot, configFileName, verbose=...)"
                   creates a runable instance. Ie. "t()" runs the show. 
  configFn     ... A method called in the robot's preconfigration mode.
  canArgs      ... Named arguments passed down to the CAN.
  '''
    if len(cmd_args) < 2:
        print __doc__
        sys.exit(-1)
    configFilename = cmd_args[1]
    logName = None
    simulation = None
    metaLog = None

    if len(cmd_args) > 2:
        if cmd_args[2] == '--file':
            logName = cmd_args[3]
            if "meta" in logName:
                runNumber = 1
                try:
                    if 'F' == cmd_args[-1] or 'FF' == cmd_args[-1]:
                        runNumber = int(cmd_args[-2])
                    else:
                        runNumber = int(cmd_args[-1])
                except:
                    pass
                print "METALOG", logName, runNumber
                metaLog = open(logName)
                for line in metaLog:
                    if line.startswith("CANlog:"):
                        runNumber -= 1
                        if runNumber <= 0:
                            logName = logName[:logName.find(
                                "meta")] + line.split()[1].split("/")[-1]
                            break
            if len(cmd_args) > 4 and cmd_args[-1] == 'FF':
                can = CAN(ReplyLogInputsOnly(logName), skipInit=True)
            else:
                assertWrite = not (len(cmd_args) > 4 and cmd_args[-1] == 'F')
                can = CAN(ReplyLog(logName, assertWrite=assertWrite),
                          skipInit=True)
        else:
            assert (cmd_args[2] == '--simul')
            simulation = True  # TODO
    else:
        metaLog = open(timeName("logs/meta", "log"), "w")
        metaLog.write(str(cmd_args) + "\n")
        metaLog.write("configFilename: '" + str(configFilename) + "'\n")
        if configFilename:
            try:
                metaLog.write("------------------\n")
                metaLog.write(open(configFilename).read())
                metaLog.write("------------------\n")
            except:
                metaLog.write("Load FAILED\n")
        metaLog.flush()

        can = CAN(**canArgs)

    isFromLog = logName is not None

    while 1:
        if not isFromLog and not simulation:
            can.relog('logs/s')
            print starter.starter(can, configFn=configFn)
            metaLog.write("CANlog:\t" + str(can.relog('logs/r')) + "\n")
            metaLog.flush()

        if simulation:
            robot = Simulator()
        else:
            robot = robotFactory(can=can, replyLog=logName, metaLog=metaLog)

        if isFromLog or simulation:
            viewlog.viewLogFile = open("view.log", "w")
            robot.addExtension(viewLogExtension)
            #      robot.addExtension( viewCompassExtension )
            robot.addExtension(viewPoseExtension)
            laserViewer = LaserViewer(robot, distanceLimit=2.0, sideLimit=10)
        if isFromLog:
            view = ViewCameraExtension(os.path.dirname(sys.argv[3]))
            robot.addExtension(view.viewCameraExtension)

        if not __run(
                robot, task, configFilename,
                verbose=(isFromLog or simulation)) or isFromLog or simulation:
            break
        del robot  # robot has switch to preoperation ... probably not a good idea
Exemplo n.º 20
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,
                  timestamps_log=open(metalog.getLog('timestamps'), 'w'))
    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

    for sensor_name in ['gps', 'laser', 'camera']:
        attach_sensor(robot, sensor_name, metalog)

    print "Wait for gas"
    while robot.canproxy.gas is None:
        robot.update()

    print "Wait for center"
    robot.canproxy.stop()
    # not available now :( ... wait_for_start(robot)

    moving = False
    robot.desired_speed = 0.5
    start_time = robot.time
    prev_gps = robot.gps_data
    prev_destination_dist = None
    last_laser_update = None
    prev_laser = None
    while robot.time - start_time < 3 * 60:  # 3min
        robot.update()
        dist = None
        turn_angle = None
        if robot.laser_data is not None:
            assert len(robot.laser_data) == 541, len(robot.laser_data)
            if robot.laser_data != prev_laser:
                prev_laser = robot.laser_data
                last_laser_update = robot.time
            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)
            turn_angle = follow_wall_angle(robot.laser_data, radius=1.5)

        if last_laser_update is not None and robot.time - last_laser_update > 0.3:
            print "!!!WARNING!!! Missing laser updates for last {:.1f}s".format(
                robot.time - last_laser_update)
            dist = None  # no longer valid distance measurements

        if robot.gps_data != prev_gps:
            if turn_angle is not None:
                print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (
                    distL, distR), math.degrees(turn_angle)
            else:
                print robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (
                    distL, distR), turn_angle
            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
        else:  # not moving
            if dist is not None and dist > SAFE_DISTANCE_GO:
                print "GO", dist
                robot.set_desired_speed(DESIRED_SPEED)
                moving = True
        if turn_angle is not None:
            turn_cmd = max(-120, min(120, 2 * math.degrees(turn_angle)))
            robot.canproxy.set_turn_raw(turn_cmd)


#            print turn_cmd
#        if not robot.buttonGo:
#            print "STOP!"
#            break
    robot.canproxy.stop_turn()
    robot.canproxy.stop()
    for i in xrange(20):
        robot.update()
    detach_all_sensors(robot)
Exemplo n.º 21
0
def drive_remotely(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

    soc = metalog.createLoggedSocket('remote', headerFormat=None)  # TODO fix headerFormat
    soc.bind( ('',PORT) )
    if not metalog.replay:
        soc.soc.setblocking(0)
        soc.soc.settimeout( SOCKET_TIMEOUT )
    
    remote_cmd_log_name = metalog.getLog('remote_cmd')
    if metalog.replay:
        robot.remote = DummySensor()
        function = SourceLogger(None, remote_cmd_log_name).get
    else:
        robot.remote = RemoteThread(soc)
        function = SourceLogger(robot.remote.get_data, remote_cmd_log_name).get

    max_speed = None

    robot.remote_data = None
    robot.register_data_source('remote', function, remote_data_extension) 
    robot.remote.start()

    robot.canproxy.stop()
    robot.canproxy.set_turn_raw(0)

    print "Waiting for remote commands ..."
    while robot.remote_data is None:
        robot.update()
    print "received", robot.remote_data.strip()

    moving = False
    turning = False
    while robot.remote_data != 'END\n':
        robot.update()
        if robot.remote_data == 'STOP\n' and (moving or turning):
            if moving:
                robot.canproxy.stop()
                print "STOP"
                moving = False
            if turning:
                robot.canproxy.stop_turn()
                print "STOP turn"
                turning = False
        elif robot.remote_data == 'UP\n' and not moving:
            if max_speed is None:
                robot.canproxy.go()
                print "GO"
            else:
                robot.set_desired_speed(max_speed)
                print "GO", max_speed
            moving = True
        elif robot.remote_data == 'DOWN\n' and not moving:
            if max_speed is None:
                robot.canproxy.go_back()
                print "GO back"
            else:
                robot.set_desired_speed(-max_speed)
                print "GO back", max_speed
            moving = True
        elif robot.remote_data == 'LEFT\n':
            robot.canproxy.set_turn_raw(200)
            print "Left"
            turning = True
        elif robot.remote_data == 'RIGHT\n':
            robot.canproxy.set_turn_raw(-200)
            print "Right"
            turning = True
        elif robot.remote_data == 'SPEED0\n':
            max_speed = None
        elif robot.remote_data.startswith('SPEED'):
            max_speed = int(robot.remote_data[len('SPEED'):])/10.0
            print "Max Speed", max_speed

    print "received", robot.remote_data.strip()

    robot.canproxy.stop_turn()
    robot.remote.requestStop()
    robot.wait(3.0)
Exemplo n.º 22
0
def self_test(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.canproxy.stop()
#    wait_for_start(robot)
    robot.set_desired_speed(0.5)
    start_time = robot.time
    robot.canproxy.set_turn_raw(0)
    robot.canproxy.go()
    start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
    arr = []
    while robot.time - start_time < 10.0:
        robot.update()
        arr.append(robot.canproxy.gas)
        dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                          - start_dist)/2.0
        if dist > 1.0:
            print "Dist OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
            break
    print dist
    robot.stop()
    robot.wait(3.0)
    dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                      - start_dist)/2.0
    print dist
    print

    robot.canproxy.go_back()
    start_time = robot.time
    start_dist = robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw
    arr = []
    while robot.time - start_time < 10.0:
        robot.update()
        arr.append(robot.canproxy.gas)
        dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                          - start_dist)/2.0
        if dist < -1.0:
            print "Dist back OK at {}s".format(robot.time - start_time), sorted(arr)[len(arr)/2]
            break
    print dist


#        print robot.time, robot.canproxy.gas
#        if not robot.buttonGo:
#            print "STOP!"
#            break
    robot.canproxy.stop_turn()
    robot.stop()
    robot.wait(3.0)
    dist = ENC_SCALE*(robot.canproxy.dist_left_raw + robot.canproxy.dist_right_raw 
                      - start_dist)/2.0
    print dist
Exemplo n.º 23
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()
Exemplo n.º 24
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, timestamps_log=open(metalog.getLog('timestamps'), 'w'))
    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

    for sensor_name in ['gps', 'laser', 'camera']:
        attach_sensor(robot, sensor_name, metalog)

    print("Wait for gas")
    while robot.canproxy.gas is None:
        robot.update()

    print("Wait for center")
    robot.canproxy.stop()
    # not available now :( ... wait_for_start(robot)

    moving = False
    robot.desired_speed = 0.5
    start_time = robot.time
    prev_gps = robot.gps_data
    prev_destination_dist = None
    last_laser_update = None
    prev_laser = None
    while robot.time - start_time < 3*60:  # 3min
        robot.update()
        dist = None
        turn_angle = None
        if robot.laser_data is not None:
            assert len(robot.laser_data) == 541, len(robot.laser_data)
            if robot.laser_data != prev_laser:
                prev_laser = robot.laser_data
                last_laser_update = robot.time
            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)
            turn_angle = follow_wall_angle(robot.laser_data, radius=1.5)

        if last_laser_update is not None and robot.time - last_laser_update > 0.3:
            print("!!!WARNING!!! Missing laser updates for last {:.1f}s".format(robot.time - last_laser_update))
            dist = None  # no longer valid distance measurements

        if robot.gps_data != prev_gps:
            if turn_angle is not None:
                print(robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), math.degrees(turn_angle))
            else:
                print(robot.time, robot.canproxy.gas, "(%.3f, %.3f)" % (distL, distR), turn_angle)
            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
        else:  # not moving
            if dist is not None and dist > SAFE_DISTANCE_GO:
                print("GO",  dist)
                robot.set_desired_speed(DESIRED_SPEED)
                moving = True
        if turn_angle is not None:
            turn_cmd = max(-120, min(120, 2*math.degrees(turn_angle)))
            robot.canproxy.set_turn_raw(turn_cmd)
#            print turn_cmd
#        if not robot.buttonGo:
#            print "STOP!"
#            break
    robot.canproxy.stop_turn()
    robot.canproxy.stop()
    for i in range(20):
        robot.update()
    detach_all_sensors(robot)