Beispiel #1
0
def main(log, readlog, only_odometry, sensorFile, odomFile, resultname, mapconfig):
    initialized = False
    sensor = None
    navigation = None
    robot = None
    server = None
    try:
        #Initialize Sensor
        if(readlog):
            sensor = FileXTION(sensorFile)
        else:
            sensor = XTION(log)

        #Initialize Slam
        if(only_odometry):
            slam = Deterministic_SLAM(sensor, mapconfig.SIZE_PIXELS, mapconfig.SIZE_METERS)
        else:
            slam = My_SLAM(sensor, mapconfig.SIZE_PIXELS, mapconfig.SIZE_METERS, random_seed = SEED)

        #Initialize Robot
        if(readlog):
            robot = FileDrone(odomFile)
        else:
            robot = NetworkVehicle(log)
            robot.initialize()

        #Open Controll and Map Server
        server = Server(slam, mapconfig.SIZE_PIXELS, robot)
        server.start()

        #Initialize Navigation
        if(not readlog):
            navigation = Navigation(slam, mapconfig, robot.getSize(), RELEVANT_LIDARS, SECURITY_DIST_MM, Commands)
            navigation.start()

        #Monitors
        scanno = 0
        dist = 0
        timePast = 0
        trajectory = []
        start_sec = time()

        #Make initial scan
        scan = sensor.scan()

        initialized = True

        #Main loop
        while(True):
            scanno += 1

            #get command
            if(readlog):
                command = None
            else:
                command = navigation.update(scan)
                
                if(command == None):
                    print "Navigation terminated."
                    break

            #send command and get odometry
            velocities = robot.move(command)

            #check if velocities are valid
            if(velocities == None):
                print "Robot terminated."
                break

            #update monitors
            dist += velocities[0]
            timePast += velocities[2]

            #get scan
            scan = sensor.scan()

            #check if scan is valid
            if(len(scan)<=0):
                print "Sensor terminated."
                break

            #Update SLAM
            slam.update(scan, velocities)

            # Get new position
            x_mm, y_mm, theta_degrees = slam.getpos()    
        
            # Add new position to trajectory
            trajectory.append((x_mm, y_mm))

    except KeyboardInterrupt:
        print "Program stoped!"
    finally:
        print "Shutting down."
        if(sensor != None): sensor.shutdown()
        if(navigation != None): navigation.stop()
        if(robot != None): robot.shutdown()
        if(server != None): server.close()

    if(initialized):
        #Print results
        elapsed_sec = time() - start_sec
        print('\n%d scans in %f sec = %f scans / sec' % (scanno, elapsed_sec, scanno/elapsed_sec))
        print ('Distance traveled:%f mm in %fs' % (dist, timePast))

        #generate map
        mapbytes = createMap(slam, trajectory, mapconfig)
        # Save map and trajectory as png file        
        mapconfig.safeaspng(mapbytes, resultname)
Beispiel #2
0
def main(log, readlog, only_odometry, sensorFile, odomFile, resultname,
         mapconfig):
    initialized = False
    sensor = None
    navigation = None
    robot = None
    server = None
    try:
        #Initialize Sensor
        if (readlog):
            sensor = FileXTION(sensorFile)
        else:
            sensor = XTION(log)

        #Initialize Slam
        if (only_odometry):
            slam = Deterministic_SLAM(sensor, mapconfig.SIZE_PIXELS,
                                      mapconfig.SIZE_METERS)
        else:
            slam = My_SLAM(sensor,
                           mapconfig.SIZE_PIXELS,
                           mapconfig.SIZE_METERS,
                           random_seed=SEED)

        #Initialize Robot
        if (readlog):
            robot = FileDrone(odomFile)
        else:
            robot = NetworkVehicle(log)
            robot.initialize()

        #Open Controll and Map Server
        server = Server(slam, mapconfig.SIZE_PIXELS, robot)
        server.start()

        #Initialize Navigation
        if (not readlog):
            navigation = Navigation(slam, mapconfig, robot.getSize(),
                                    RELEVANT_LIDARS, SECURITY_DIST_MM,
                                    Commands)
            navigation.start()

        #Monitors
        scanno = 0
        dist = 0
        timePast = 0
        trajectory = []
        start_sec = time()

        #Make initial scan
        scan = sensor.scan()

        initialized = True

        #Main loop
        while (True):
            scanno += 1

            #get command
            if (readlog):
                command = None
            else:
                command = navigation.update(scan)

                if (command == None):
                    print "Navigation terminated."
                    break

            #send command and get odometry
            velocities = robot.move(command)

            #check if velocities are valid
            if (velocities == None):
                print "Robot terminated."
                break

            #update monitors
            dist += velocities[0]
            timePast += velocities[2]

            #get scan
            scan = sensor.scan()

            #check if scan is valid
            if (len(scan) <= 0):
                print "Sensor terminated."
                break

            #Update SLAM
            slam.update(scan, velocities)

            # Get new position
            x_mm, y_mm, theta_degrees = slam.getpos()

            # Add new position to trajectory
            trajectory.append((x_mm, y_mm))

    except KeyboardInterrupt:
        print "Program stoped!"
    finally:
        print "Shutting down."
        if (sensor != None): sensor.shutdown()
        if (navigation != None): navigation.stop()
        if (robot != None): robot.shutdown()
        if (server != None): server.close()

    if (initialized):
        #Print results
        elapsed_sec = time() - start_sec
        print('\n%d scans in %f sec = %f scans / sec' %
              (scanno, elapsed_sec, scanno / elapsed_sec))
        print('Distance traveled:%f mm in %fs' % (dist, timePast))

        #generate map
        mapbytes = createMap(slam, trajectory, mapconfig)
        # Save map and trajectory as png file
        mapconfig.safeaspng(mapbytes, resultname)
Beispiel #3
0
def main(g=0.4, h=0.4):

    filename = 'map_%f_%f' % (g, h)
    """
    if(use_odometry):
        filename += 'withodometry_'
    if(readlog):
        filename += 'fromlog_'
    if(seed==0):
        filename += 'deterministic'
    else:
        filename += ('rmhc_seed' + str(seed))
    """

    #initialize the asus xtion as sensor
    if (readlog):
        sensor = FileXTION("log")
    else:
        sensor = XTION()  #NetworkSensor() #

    # Create a CoreSLAM object with laser params and optional robot object
    slam = My_SLAM(sensor, MAP_SIZE_PIXELS, MAP_SIZE_METERS, random_seed=seed, g=g, h=h) \
        if seed \
        else Deterministic_SLAM(sensor, MAP_SIZE_PIXELS, MAP_SIZE_METERS)

    robot = None

    #initialiye robot
    if (use_odometry):
        navigation = Navigation(slam, MapConfig(), ROBOT_SIZE_METERS, 50, 800,
                                Commands)

        if (readlog):
            robot = FileDrone("odometry")
        else:
            robot = NetworkVehicle()  #Drone()
            robot.initialize()
    if (stream):
        server = Server(slam, MAP_SIZE_PIXELS, robot)
        server.start()

    # Start with an empty trajectory of positions
    trajectory = []

    # Start timing
    start_sec = time()

    # Loop
    atexit.register(set_normal_term)
    set_curses_term()

    scanno = 0

    dist = 0
    zeit = 0

    if (use_odometry): navigation.start()

    ##make initial scan
    scan = sensor.scan()

    while (True):
        scanno += 1
        if use_odometry:
            ##navigaiton

            command = navigation.update(scan)
            #print command

            ##odometry
            velocities = robot.move(command)
            dist += velocities[0]
            zeit += velocities[2]
            print velocities
            ##lidar
            scan = sensor.scan()

            if (len(scan) <= 0):
                print 'Reader error or end of file.'
                break

            # Update SLAM with lidar and velocities
            slam.update(scan, velocities)

        else:
            scan = sensor.scan()
            if (len(scan) <= 0):
                print 'Reader error or end of file.'
                break

            # Update SLAM with lidar alone
            slam.update(scan)

        # Get new position
        x_mm, y_mm, theta_degrees = slam.getpos()

        # Add new position to trajectory
        trajectory.append((x_mm, y_mm))

        if kbhit():
            break

    if (use_odometry):
        robot.shutdown()
        navigation.stop()

    # Report elapsed time
    elapsed_sec = time() - start_sec
    print('\n%d scans in %f sec = %f scans / sec' %
          (scanno, elapsed_sec, scanno / elapsed_sec))

    print('dist traveled:%f mm in %fs' % (dist, zeit))

    mapbytes = createMap(slam, trajectory)

    # Save map and trajectory as PGM file
    pgm_save(filename, mapbytes, (MAP_SIZE_PIXELS, MAP_SIZE_PIXELS))

    image = cv2.imread(filename, 0)
    print "Accessing the image.. again. So dirty."
    print "Saving as .png: ..."
    cv2.imwrite("test.png", image)

    if (stream):
        server.close()
    print "done"