Exemple #1
0
 def connect(self):
     if self.session:
         return
     try:
         self.session = GPS(mode=WATCH_ENABLE)
     except OSError:
         return
Exemple #2
0
 def testCarRepair(self):
     gps = GPS(state=[
         'son-at-home', 'car-needs-battery', 'have-money', 'have-phone-book'
     ],
               goals=['son-at-school'],
               ops=self.school_ops)
     self.assertEqual(gps.solve(), 'solved')
Exemple #3
0
 def __init__(self):
     super(EventProcessor, self).__init__()
     self.message_handler = MessageHandler()
     self.gps = GPS()
     Message.init()
     Detector.start_plugins()
     Reactor.add_plugin_events()
Exemple #4
0
def drive_instructions(picar: Car, nav: GPS, instructions: deque):

    # while not at target
    while (len(instructions) > 0):
        # convert instructions to polar
        step = instructions.popleft()
        print("directions: ", step)

        direction = step[0] - picar.orientation
        direction = (direction + 180) % 360 - 180
        #print("turning angle", direction)

        driven = 0
        # change direction if needed
        if direction > 0:
            picar.turn_right(direction)
        elif direction < 0:
            picar.turn_left(abs(direction))

        if step[1] >= 0:
            driven = picar.drive_forward(distance=step[1])
        else:
            driven = picar.drive_backward(distance=abs(step[1]))

        nav.update_postion(distance=int(driven), orientation=picar.orientation)
        print("curr position", nav.position)

        # if blocked rerout
        if driven < step[1]:
            return False

    return True
Exemple #5
0
def start():
    # 启动GPS
    GPS_thread = GPS()
    GPS_thread.start()
    # 启动提示预警
    remind_thread = remind.remind_thread()
    remind_thread.start()
Exemple #6
0
def main( controller_type, training_data, policy_file, gps_params = gps_params_default ):
	
	sys.path.append( gps_params[ 'caffe_path' ] )
	
	# create controller
	if controller_type == 'cartpole':
		C = controller.CartPole()
		C.set_system_cost( x0=np.array( [ 0, 0, 0, 0 ] ), u0=0.0, Wx=np.eye( 4 ), Wu=1e-3 )
	else:
		print( 'not implemented: ' + controller_type )
	
	x_len = C.get_x_len()
	u_len = C.get_u_len()
	gps_params[ 'x_len' ] = x_len
	gps_params[ 'u_len' ] = u_len

	# load training data
	f = h5py.File( 'data.h5', 'r' )
	xu_train_orig = f[ 'x' ].value
	
#	if 'o' not in f:
#		print( 'WARNING: observations not in data' )
#		o_train_orig = None
#	else:
#		o_train_orig = f[ 'o' ]
#		o_train_orig = o_train_orig[ ::gps_params[ 'resample' ]	]
	o_train = None

	if xu_train_orig.ndim == 2:
		xu_train_orig = xu_train_orig[ :, 1: ]	# TODO - at the moment 1st column is time

		s = xu_train_orig.shape
		xu_train_orig = xu_train_orig.reshape( (1,s[0], s[1]) )

	resample_idx = range( 0, xu_train_orig.shape[1], gps_params[ 'resample' ] )

	xu_train_orig = np.take( xu_train_orig, resample_idx, axis=1 )
	xu_train = xu_train_orig
	x_train = xu_train_orig[ :,:, :x_len ]
	u_train = xu_train_orig[ :,:, x_len ]			# TODO check table organisation

	gps = GPS( gps_params )
	training_errors = []
		
	# loop untill convergence:
	for k in range( gps_params[ 'K'] ):
		print( 'running for k: {0}'.format( k ) )

		# execute gps
		policy, training_error = gps.train( xu_train, o_train, C.get_system_cost(), gps_params )
		training_errors.append( training_error )

		# run on controller and collect the data
		xu_run = C.run( policy )

		# merge data
		xu_train = gps.merge_data( xu_train, o_train, xu_run, o_run )
Exemple #7
0
    def __init__(self,
                 *,
                 port: str,
                 baudrate: int,
                 path: str = 'db',
                 bs: int = 32,
                 to: int = 5,
                 http: int = 8080):

        super().__init__()
        pid = os.getpid()
        logger.info('*** NMEA Recorder startup (%d)' % pid)

        self.process: Dict[str, int] = {}
        self.process['main'] = pid

        self.ppp: Dict[str, Patrol] = {}
        self.ppp['main'] = Patrol(pid=pid)

        self.ready: bool = True
        self.g = GPS()
        self.ws = WebsocketServer(debug=True)

        self.qp = Queue()

        self.dbsession = DBSession(path=pathlib.Path(path),
                                   buffersize=bs,
                                   timeout=to)
        self.dbsession.start()
        self.process[self.dbsession.name] = self.dbsession.pid
        self.ppp[self.dbsession.name] = Patrol(pid=self.dbsession.pid)

        self.receiver = Receiver(port=port, baudrate=baudrate, qp=self.qp)
        self.receiver.start()
        self.process[self.receiver.name] = self.receiver.pid
        self.ppp[self.receiver.name] = Patrol(pid=self.receiver.pid)

        self.mc = fromUDP(quePoint=self.qp, mcip='239.192.0.1', mcport=60001)
        self.mc.start()
        self.process[self.mc.name] = self.mc.pid
        self.ppp[self.mc.name] = Patrol(pid=self.mc.pid)

        self.main = Thread(target=self.collector, name='MainLoop', daemon=True)
        self.main.start()

        self.p = Thread(target=self.patrol, name='Patrol', daemon=True)
        self.p.start()
        for k, v in self.ppp.items():
            v.start()

        self.add_route('/ws', self.ws.wsserver, websocket=True)
        self.add_route('/', self.top)
        self.add_route('/main.js', self.mainJS)
        self.add_route('/classes.js', self.classes)

        self.add_event_handler('shutdown', self.cleanup)  # notice!
        self.run(address='0.0.0.0', port=http)
Exemple #8
0
def gps_main():
    gps = GPS()
    speed = 0

    #makes data a global so newest data point can still be sent
    global data

    #adjusts GPS acquisition based on speed of target
    while True:

        if (speed < 5):
            time.sleep(30)
        elif ((speed >= 5) and (speed < 20)):
            time.sleep(5)
        elif ((speed >= 20) and (speed < 45)):
            time.sleep(10)
        elif ((speed >= 45) and (speed < 70)):
            time.sleep(20)
        elif (speed >= 70):
            time.sleep(40)

        # Sample GNSS data
        lck.acquire()  #locks other threads out
        GPSdata = gps.get_RMCdata(defaultLogger)
        lck.release()  #releases back to other threads

        # Store the data
        if not (GPSdata == {}):

            #converts speed to mph from knots
            speed = GPSdata['speed'] * 1.15078
            print("speed={}".format(speed))

            # Use when we support sending speed to host
            #data = ''
            #for val in GPSdata.values():
            #    data = "{0}{1}".format(data, val)
            #    data = '{},'.format(data)
            lck.acquire()  #locks other threads out
            data = "{0},{1},{2},{3},".format(GPSdata['time'],
                                             GPSdata['latitude'],
                                             GPSdata['longitude'],
                                             GPSdata['date'])
            print(data)
            archiveLogger.write(data)
            unsentLogger.write("{0}{1}".format(len(data), data))
            defaultLogger.info(data)
            lck.release()  #releases files to others threads
        else:
            #TODO: remove print
            print("No GPS data.")
            lck.acquire()
            defaultLogger.info("No GPS data.")
            data = ""
            lck.release()
Exemple #9
0
def atestempty():
    grid = np.zeros([20, 20], dtype=int)

    t = GPS()
    t.load_grid(grid, start_x=10, start_y=0)
    instructions = t.set_navigation_goal((19, 10))

    # while not at target
    while (len(instructions) > 0):
        # convert instructions to polar
        step = instructions.pop()
        print(step)
class IpLocator:
    def __init__(self):
        self._geo_ip = GeoIP.open("%s/GeoLiteCity.dat" % os.path.dirname(__file__),
                                  GeoIP.GEOIP_STANDARD)
        self._gps = GPS()

    def locate(self, addr):
        gir = self._geo_ip.record_by_addr(addr)
        if gir:
            x = self._gps.x(gir['longitude'])
            y = self._gps.y(gir['latitude'])
            return x, y
Exemple #11
0
def stationary_scan_test():
    scan_list = scanner.scan_step_dist()

    while not scan_list:
        scan_list = scanner.scan_step_dist()

    print(scan_list)

    grid = np.zeros([50, 50], dtype=int)

    t = GPS()
    t.load_grid(grid, resolution=2, start_x=0, start_y=0)

    # performs a full 180 deg scan at 5 deg intervals
    obstacles = scanner.mapping_scan()
    print(obstacles)
    # populate map with obstacles
    for obst in obstacles:
        picar_orientation = 0

        # actual orientation = picar_orientation + obstacle_scan angle
        orientation = obst[0] + picar_orientation

        t.add_relative_obstacle(orientation=orientation, distance=obst[1])

    # save the scan results to file
    t.save_grid('maps/1object_scan_result.out')

    return t
 def __init__(self):
     self.logger = Logger()
     self.logger.write("Robot: initializing")
     self.logger.display("Starting...")
     ports = usb_probe.probe()
     self.logger.write("Robot: found USB ports...")
     for port in ports:
         self.logger.write("       %s, %s" % (ports[port], port))
     self.speedometer = Speedometer(ports['speedometer'], 9600, self.logger)
     self.compasswitch = Compasswitch(ports['compasswitch'], 9600,
                                      self.logger)
     self.powersteering = PowerSteering(ports['chias'], 9600, self.logger,
                                        self.speedometer, self.compasswitch)
     self.gps = GPS(ports['gps'], 4800, self.logger)
     self.camera = Camera(9788, self.logger)
Exemple #13
0
 def __init__(self):
     super(EventProcessor, self).__init__()
     self.message_handler = MessageHandler()
     self.gps = GPS()
     Message.init()
     Detector.start_plugins()
     Reactor.add_plugin_events()
Exemple #14
0
 def connect(self):
     if self.session:
         return
     try:
         self.session = GPS(mode=WATCH_ENABLE)
     except OSError:
         return
Exemple #15
0
def astest():

    t = GPS(map_width=100,
            map_length=100,
            resolution=10,
            start_x=50,
            start_y=0)
    target = (50, 20)

    obstacles = scanner.mapping_scan()
    print(obstacles)
    #obstacles[:,0] *= -1
    #print(obstacles)

    # populate map with obstacles
    for obst in obstacles:
        picar_orientation = 0

        # actual orientation = picar_orientation + obstacle_scan angle
        orientation = obst[0] + picar_orientation

        t.add_relative_obstacle(orientation=obst[0], distance=obst[1])

    t.save_grid('maps/1object_scan_result.out')

    instructions = t.set_navigation_goal(target)

    # while not at target
    while (len(instructions) > 0):
        # convert instructions to polar
        step = instructions.pop()
        print(step)
Exemple #16
0
    def __init__(self, config):
        self.config = config

        gps = GPS(config)
        measure = Measure()

        self.mobile = Mobile(gps, measure, config)

        self.step = config['step']
 def get_intersection(self):
     coor, old_coor = GPS.get_gps_all(color=self.color)
     for origin, turn in self.intersections:
         if self._within_box(origin, coor):
             if time.time() < self.wait_period + self.time_stamp:
                 return False, None
             else:
                 self.time_stamp = time.time()
                 return True, turn
     return False, None
 def get_intersection_old(self):
     coor = GPS.get_gps(color=self.color)
     for origin, action in self.intersections:
         if self._within_box(origin, coor):
             if time.time() < self.wait_period + self.time_stamp:
                 return False, "Too Soon", coor
             else:
                 self.time_stamp = time.time()
                 return True, action, coor
     return False, "No Intersection", coor
Exemple #19
0
 def __init__(self, args):
     self._ip_locator = ip_locator.IpLocator()
     self._gps = GPS(WORLD_WIDTH, WORLD_HEIGHT)
     self._here_x = self._gps.x(HERE_LONGITUDE)
     self._here_y = self._gps.y(HERE_LATITUDE)
     self._load_locations()
     visualizer.Visualizer.__init__(self, args)
     self._set_camera_position(CAMERA_POSITION)
     self._set_camera_orientation(CAMERA_Y_ORIENTATION, CAMERA_X_ORIENTATION)
     self._world = world.World(WORLD_WIDTH, WORLD_HEIGHT)
     self.enable_3d()
Exemple #20
0
 def __init__(self, args):
     self._gps = GPS(WORLD_WIDTH, WORLD_HEIGHT)
     self._here_x = self._gps.x(HERE_LONGITUDE)
     self._here_y = self._gps.y(HERE_LATITUDE)
     self._load_locations()
     visualizer.Visualizer.__init__(self, args, file_class=File)
     self._set_camera_position(CAMERA_POSITION)
     self._set_camera_orientation(CAMERA_Y_ORIENTATION, CAMERA_X_ORIENTATION)
     self._world = world.World(WORLD_WIDTH, WORLD_HEIGHT)
     self.enable_3d()
     self.playing_segments = collections.OrderedDict()
class Map:
    """
    Handles the map of the world.
    On a global scale: has a start, destination, and a list of waypoints. Coord system: GPS coordinates
    On a local scale: has a 2D polar map of the world around the boat. Coord system: moves and oriented with the boat
    """
    def __init__(self):
        self.wind_vane = WindVane()
        self.gps = GPS()
        self.lidar = Lidar()
        self.world_map = load_world_map()

    def update_surroundings(self):
        """
        Updates the computer's knowledge of the surroundings.
        :return:
        """
        self.wind_vane.read_wind_vane()
        self.gps.read_gps()
        self.lidar.read_lidar()
Exemple #22
0
def drive_target(nav: GPS, target: tuple):

    car_theta = 0
    curr_distance = 0
    picar = Car()

    at_destination = False

    while (not at_destination):

        # scan for obstacles
        obstacles = scanner.mapping_scan()
        print(obstacles)

        for obst in obstacles:
            abs_orient = obst[0] + picar.orientation
            nav.add_relative_obstacle(orientation=abs_orient, distance=obst[1])

        instructions = nav.set_navigation_goal(target)

        at_destination = drive_instructions(picar, nav, instructions)
Exemple #23
0
    def get_turn_old(self):
        if self.destination is None:
            raise Exception("Destination Point not set")

        current_quad = self._get_quadrant(GPS.get_gps())

        if self.cv.next(current_quad) == self.destination_quad:
            return "LEFT"
        elif self.cv.previous(current_quad) == self.destination_quad:
            return "RIGHT"
        else:
            return "STRAIGHT"
Exemple #24
0
class IpLocator:
    def __init__(self):
        self._geo_ip = GeoIP.open("%s/GeoLiteCity.dat" % os.path.dirname(__file__),
                                  GeoIP.GEOIP_STANDARD)
        self._gps = GPS()

    def locate(self, addr):
        record = self._geo_ip.record_by_addr(addr)
        if record:
            if record['city']:
                return self._location_tuple_from_record(record, coding="unicode_escape")
            else:
                extension_record = self._look_up_in_db_extension(addr)
                if extension_record:
                    return self._location_tuple_from_record(extension_record)
                else:
                    print "WARNING: unknown city for IP %s (GeoIP reports coordinates %r, %r)" % (
                        addr, record['longitude'], record['latitude'])
                    print "http://www.ip-tracker.org/locator/ip-lookup.php?ip=%s" % addr
                    return self._location_tuple_from_record(record)
        else:
            print "WARNING: unknown location IP %s" % addr

    def _look_up_in_db_extension(self, addr):
        addr_without_last_part = self._strip_last_addr_part(addr)
        try:
            return DB_EXTENSION[addr_without_last_part]
        except KeyError:
            return None

    def _strip_last_addr_part(self, addr):
        return ".".join(addr.split(".")[0:3]) + "."

    def _location_tuple_from_record(self, record, coding=None):
        x = self._gps.x(record['longitude'])
        y = self._gps.y(record['latitude'])
        place_name = record['city']
        if place_name and coding:
            place_name = place_name.decode(coding)
        return x, y, place_name
Exemple #25
0
def drive_picar():
    nav = GPS(map_width=100,
              map_length=100,
              resolution=10,
              start_x=50,
              start_y=0)
    target = (50, 15)

    c = PiCar(nav)

    c.drive_target(target)

    del c
    def _near_destination(self):
        current_coor = GPS.get_gps()
        x = current_coor[0]
        y = current_coor[1]

        if x <= 0.0 or y <= 0.0:
            return False

        x_dif = abs(self.destination[0] - x)
        y_dif = abs(self.destination[1] - y)
        if x_dif < self.box_width and y_dif < self.box_height:
            return True
        else:
            return False
Exemple #27
0
class GPSPoller(threading.Thread):

    def __init__(self):
        threading.Thread.__init__(self)
        self.session = None
        self.current_value = Location()
        self._stopped = False
        self._skipped = 0

    def connect(self):
        if self.session:
            return
        try:
            self.session = GPS(mode=WATCH_ENABLE)
        except OSError:
            return

    def get_location(self):
        return self.current_value

    def stop(self):
        self._stopped = True

    def run(self):
        while not self._stopped:
            self.current_value.check()
            self.connect()
            if not self.session:
                time.sleep(1)
                continue
            try:
                v = self.session.next()
                if 'lat' in dict(v):
                    alt = None
                    if 'alt' in dict(v):
                        alt = v['alt']
                    self.current_value.set(v['lat'], v['lon'], v['speed'], alt)
                self._skipped = 0
            except StopIteration:
                self._skipped += 1
                if self._skipped > 20:
                    self.session = None
                time.sleep(0.5)
            except client:
                logger.exception("Client exception")
                time.sleep(0.5)
            except Exception:
                logger.exception("Unknown exception")
                time.sleep(0.5)
Exemple #28
0
class GPSPoller(threading.Thread):
    def __init__(self):
        threading.Thread.__init__(self)
        self.session = None
        self.current_value = Location()
        self._stopped = False
        self._skipped = 0

    def connect(self):
        if self.session:
            return
        try:
            self.session = GPS(mode=WATCH_ENABLE)
        except OSError:
            return

    def get_location(self):
        return self.current_value

    def stop(self):
        self._stopped = True

    def run(self):
        while not self._stopped:
            self.current_value.check()
            self.connect()
            if not self.session:
                time.sleep(1)
                continue
            try:
                v = self.session.next()
                if 'lat' in dict(v):
                    alt = None
                    if 'alt' in dict(v):
                        alt = v['alt']
                    self.current_value.set(v['lat'], v['lon'], v['speed'], alt)
                self._skipped = 0
            except StopIteration:
                self._skipped += 1
                if self._skipped > 20:
                    self.session = None
                time.sleep(0.5)
            except client:
                logger.exception("Client exception")
                time.sleep(0.5)
            except Exception:
                logger.exception("Unknown exception")
                time.sleep(0.5)
Exemple #29
0
class EventProcessor(Component):
    _singleton = None

    def __new__(cls, *args, **kwargs):
        if not cls._singleton:
            cls._singleton = super(EventProcessor, cls).__new__(cls)
        return cls._singleton

    def __init__(self):
        super(EventProcessor, self).__init__()
        self.message_handler = MessageHandler()
        self.gps = GPS()
        Message.init()
        Detector.start_plugins()
        Reactor.add_plugin_events()

    def react_internal(self, event):
        alert = Alert(event)
        Reactor.react(alert)

    def compute_distance(self, location):
        current_location = self.gps.get_current_coordinates()
        return GPS.distance_between(current_location, location)

    def react_external(self, event):
        distance = self.compute_distance(event.location)
        alert = Alert(event, distance)
        Reactor.react(alert)

    @handler("msg_received")
    def msg_received(self, *args):
        self.react_external(args[0])

    @handler("detection_received")
    def detection_received(self, *args):
        event = args[0]
        if event.own_warning:
            self.react_internal(event)
        self.message_handler.emit_event(event)

    @classmethod
    def handle_detection(cls, event):
        cls._singleton.fire(detection_received(event))

    @classmethod
    def handle_message(cls, event):
        cls._singleton.fire(msg_received(event))
Exemple #30
0
class EventProcessor(Component):
    _singleton = None

    def __new__(cls, *args, **kwargs):
        if not cls._singleton:
            cls._singleton = super(EventProcessor, cls).__new__(cls)
        return cls._singleton

    def __init__(self):
        super(EventProcessor, self).__init__()
        self.message_handler = MessageHandler()
        self.gps = GPS()
        Message.init()
        Detector.start_plugins()
        Reactor.add_plugin_events()

    def react_internal(self, event):
        alert = Alert(event)
        Reactor.react(alert)

    def compute_distance(self, location):
        current_location = self.gps.get_current_coordinates()
        return GPS.distance_between(current_location, location)

    def react_external(self, event):
        distance = self.compute_distance(event.location)
        alert = Alert(event, distance)
        Reactor.react(alert)

    @handler("msg_received")
    def msg_received(self, *args):
        self.react_external(args[0])

    @handler("detection_received")
    def detection_received(self, *args):
        event = args[0]
        if event.own_warning:
            self.react_internal(event)
        self.message_handler.emit_event(event)

    @classmethod
    def handle_detection(cls, event):
        cls._singleton.fire(detection_received(event))

    @classmethod
    def handle_message(cls, event):
        cls._singleton.fire(msg_received(event))
Exemple #31
0
    def __init__(self, master):
        threading.Thread.__init__(self)
        self.master = master
        master.title("Controller for Walkera drones")
        self.setWindowSize(350, 600)
        
        self.GPS = GPS(0, 0, 1) # lat, lng, caution distance in metres
        self.flightModePresets = FMP()
        self.commander = None
        self.video = None
        self.flightMode = None
        
        self.throttleVal = IntVar()
        self.rotationVal = IntVar()
        self.elevVal = IntVar()
        self.aileVal = IntVar()
        self.statusVar = StringVar()
        
        self.statusVar.set("STATUS: Disconnected")
        
        main_panel = PanedWindow(self.master)
        main_panel.pack(fill=BOTH, expand=1)

        statusStrLabel = Label(main_panel, textvariable= self.statusVar)
        statusStrLabel.pack()
        
        self.addScales(main_panel)
        
        buttonFrame = Frame(main_panel)
        buttonFrame.pack()
        
        self.connectButton = Button(buttonFrame, text="Connect", command=self.connectDrone, fg="green")
        self.disconnectButton = Button(buttonFrame, text="Disconnect", command=self.disconnectDrone, fg="red", state=DISABLED)
        
        self.connectButton.pack(side = LEFT)
        self.disconnectButton.pack(side = LEFT)
        
        presetFrame = Frame(main_panel)
        presetFrame.pack()
                
        self.videoImage = Label(presetFrame, width=352, height=288)
        self.videoImage.pack(side=BOTTOM, padx=10, pady=10)
Exemple #32
0
 def attachGPS(self):
     if self.replyLog is None:
         self.gps = GPS(verbose=0)
         name = timeName("logs/src_gps_", "log")
         if self.metaLog:
             self.metaLog.write("GPSLOG:\t" + name + "\n")
             self.metaLog.flush()
         self.registerDataSource('gps',
                                 SourceLogger(self.gps.coord, name).get)
     else:
         self.gps = DummyGPS()
         if self.metaLog:
             gpsSrcLog = self.metaLogName("GPSLOG:")
         else:
             gpsSrcLog = self.replyLog[:-18] + "src_gps_" + self.replyLog[
                 -17:]
         print "GPSLOG:", gpsSrcLog
         self.registerDataSource('gps', SourceLogger(None, gpsSrcLog).get)
     self.gpsData = None
     self.addExtension(gpsDataExtension)
Exemple #33
0
def main():
    dt = 0.01

    LEFT_MOTOR_INPUT = (17, 27)
    RIGHT_MOTOR_INPUT = (5, 6)

    LEFT_ENCODER_INPUT = {'hall_sensor_A': 23, 'hall_sensor_B': 24, 'ticks_per_revolution': 2400}
    RIGHT_ENCODER_INPUT = {'hall_sensor_A': 13, 'hall_sensor_B': 19, 'ticks_per_revolution': 2350}

    left_motor = Motor(LEFT_MOTOR_INPUT[0], LEFT_MOTOR_INPUT[1])
    right_motor = Motor(RIGHT_MOTOR_INPUT[0], RIGHT_MOTOR_INPUT[1])

    left_encoder = QuadratureEncoder(LEFT_ENCODER_INPUT['ticks_per_revolution'], LEFT_ENCODER_INPUT['hall_sensor_A'], LEFT_ENCODER_INPUT['hall_sensor_B'])
    right_encoder = QuadratureEncoder(RIGHT_ENCODER_INPUT['ticks_per_revolution'], RIGHT_ENCODER_INPUT['hall_sensor_A'], RIGHT_ENCODER_INPUT['hall_sensor_B'])

    gps = GPS()
    magnetometer = Magnetometer()

    robot = Robot(left_motor, right_motor, left_encoder, right_encoder, gps, magnetometer)
    controller = PIDController(robot)

    target = SphericalPoint(-12.016592, -77.049883 )

#    target = SphericalPoint(-12.016679333333334,-77.05032666666666)
#    target = SphericalPoint(-12.016824833333333,-77.04993633333333)
#    target = SphericalPoint(-12.016822250210852, -77.04970762346649


    rover_manager = RoverManager(robot, controller, target)

    print(target.toENU(robot.reference))

#    rover_image_manager = ImageProcessing(robot)
#    rover_image_manager.execute()

    epoch = 0
    while(True):
        print("epoch: %d" %epoch)
        gps_enabled = (epoch > 0 and (epoch % 100 == 0))
        rover_manager.execute_with_filter(gps_enabled = gps_enabled)
        epoch += 1
Exemple #34
0
def talker():

    rospy.init_node('GPS_Streamer')
    pub = rospy.Publisher("GPS_Data",
                          mavric.msg.GPS,
                          queue_size=10,
                          latch=True)
    r = rospy.Rate(10)
    gps = GPS('/dev/serial0')
    gps.begin()
    while not rospy.is_shutdown():
        print(gps._data)
        gps.update()
        pub.publish(gps.good_fix, gps.latitude, gps.longitude, gps.altitude,
                    gps.speed, gps.heading, gps.satellites)
        r.sleep()
Exemple #35
0
 def __init__(self):
     super().__init__()
     self.nav = GPS(map_width = 100, map_length = 100, resolution = 10, start_x = 50, start_y = 0)
     self.obstacle_queue = Queue()
     self.cam = detect.TrafficCam()
Exemple #36
0
class SimplePiCar(Car):
    def __init__(self):
        super().__init__()
        self.nav = GPS(map_width = 100, map_length = 100, resolution = 10, start_x = 50, start_y = 0)
        self.obstacle_queue = Queue()
        self.cam = detect.TrafficCam()


    def drive_step(self, distance: int, power: int = 2):
        self.trip_meter.reset()

        while(self.cam.detect_traffic() == True):
            fc.stop()
            print("Obstacle detected")
            time.sleep(2)
        
        while(self.trip_meter.distance < distance):

            fc.forward(2)

        fc.stop()
        print(self.trip_meter.distance, distance)

    # drives towards a target
    def drive_target(self, target: tuple):
        
        at_destination = False

        while(target != self.nav.position):

            self.nav.clear_grid()
            # scan for obstacles
            obstacles = scanner.mapping_scan()
            print(self.orientation)
            print(obstacles)

            for obst in obstacles:
                abs_orient = obst[0] + self.orientation
                self.nav.add_relative_obstacle(orientation = abs_orient, distance = obst[1])
            

            instructions = self.nav.set_navigation_goal(target)
            
            if len(instructions) == 0:
                print("unable to find path")
                return
            # take the first 3 instructions
            
            print(target, self.nav.position)
            self.execute_instructions(instructions)

        print("arrived at destination")

    # drive according to instructions until blocked or finished
    def execute_instructions(self, instructions:deque):

        # while not at target
        steps_taken = 0
        while(len(instructions) > 0 and steps_taken< 3):
            # convert instructions to polar
            step = instructions.popleft()
            print("directions: ", step)
                    
            # calculate the shortest angle to turn
            direction = step[0] - self.orientation
            direction = (direction + 180) % 360 - 180
            #print("turning angle", direction)

            # change direction if needed
            if direction > 0: 
                self.turn_right(direction)
            elif direction < 0:
                self.turn_left(abs(direction))

            steps_taken += 1
            self.drive_step(distance = step[1])

            # update position
            self.nav.update_postion(step[1], self.orientation)
 def __init__(self):
     self.wind_vane = WindVane()
     self.gps = GPS()
     self.lidar = Lidar()
     self.world_map = load_world_map()
Exemple #38
0
 def testNoPhoneBook(self):
     gps = GPS(state=['son-at-home', 'car-needs-battery', 'have-money'],
               goals=['son-at-school'],
               ops=self.school_ops)
     self.assertEqual(gps.solve(), "can't solve")
Exemple #39
0
 def wait_input(self):
     text = input("> ")
     print("RECEIVED INPUT")
     return Event(GPS.to_tuple("41.559437 -8.403232"), 1, "input", False, time.time(), 10000000, text )
Exemple #40
0
 def testCarWorks(self):
     gps = GPS(state=['son-at-home', 'car-works'],
               goals=['son-at-school'],
               ops=self.school_ops)
     self.assertEqual(gps.solve(), 'solved')
Exemple #41
0
 def testCarRepair(self):
     gps = GPS(state=['son-at-home', 'car-needs-battery', 'have-money', 'have-phone-book'],
               goals=['son-at-school'],
               ops=self.school_ops)
     self.assertEqual(gps.solve(), 'solved')
Exemple #42
0
 def compute_distance(self, location):
     current_location = self.gps.get_current_coordinates()
     return GPS.distance_between(current_location, location)
Exemple #43
0
class Geography(visualizer.Visualizer):
    def __init__(self, args):
        self._gps = GPS(WORLD_WIDTH, WORLD_HEIGHT)
        self._here_x = self._gps.x(HERE_LONGITUDE)
        self._here_y = self._gps.y(HERE_LATITUDE)
        self._load_locations()
        visualizer.Visualizer.__init__(self, args, file_class=File)
        self._set_camera_position(CAMERA_POSITION)
        self._set_camera_orientation(CAMERA_Y_ORIENTATION, CAMERA_X_ORIENTATION)
        self._world = world.World(WORLD_WIDTH, WORLD_HEIGHT)
        self.enable_3d()
        self.playing_segments = collections.OrderedDict()
        #self._load_traces()

    def InitGL(self):
        visualizer.Visualizer.InitGL(self)
        glClearColor(0.0, 0.0, 0.0, 0.0)
        self._stable_layer = self.new_layer(self._render_world_and_history)

    def _load_traces(self):
        #f = open("sessions/120827-084403-TDL4/traces.data", "r")
        f = open("sessions/121104-171222-TDL4-slow/traces.data", "r")
        self._traces = cPickle.load(f)
        f.close()

    def _load_locations(self):
        self._locations = []
        self._grid = numpy.zeros((LOCATION_PRECISION, LOCATION_PRECISION), int)
        self._add_peers_from_log()

    def _add_peers_from_log(self):
        for location in locations.get_locations():
            self._add_location(location)

    def _add_location(self, location):
        if location and location not in self._locations:
            self._locations.append(location)
            x, y = location
            nx = int(LOCATION_PRECISION * x)
            ny = int(LOCATION_PRECISION * y)
            self._grid[ny, nx] += 1
            return True

    def _addr_location(self, addr):
        location = self._ip_locator.locate(addr)
        if location:
            x, y = location
            nx = int(LOCATION_PRECISION * x)
            ny = int(LOCATION_PRECISION * y)
            return x, y, nx, ny

    def added_segment(self, segment):
        self._add_location(self.peers_by_addr[segment.peer.addr].location)
        self.playing_segments[segment.id] = segment

    def render(self):
        glEnable(GL_LINE_SMOOTH)
        glHint(GL_LINE_SMOOTH_HINT, GL_NICEST)
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
        self._stable_layer.draw()
        self._render_grid_activity()
        #self._render_active_traces()

    def _render_world_and_history(self):
        self._location_max_value = numpy.max(self._grid)
        #self._render_world()

        self._render_bar_grid_lines()
        self._render_bar_grid_points()

        #self._render_parabolae()
        #self._render_land_points()

        #self._render_locations()

    def update(self):
        self._grid_activity = numpy.zeros((LOCATION_PRECISION, LOCATION_PRECISION), int)
        outdated = []
        for segment in self.playing_segments.values():
            if segment.is_playing():
                if segment.peer.location:
                    x, y = segment.peer.location
                    nx = int(LOCATION_PRECISION * x)
                    ny = int(LOCATION_PRECISION * y)
                    self._grid_activity[ny, nx] = 1
            else:
                outdated.append(segment.id)

        if len(outdated) > 0:
            for segment_id in outdated:
                del self.playing_segments[segment_id]

    def _render_world(self):
        glColor3f(*LAND_COLOR)

        for path in self._world.paths:
            self._render_land(path)

    def _render_land(self, path):
        glBegin(GL_LINE_STRIP)
        for x, y in path:
            glVertex3f(x, 0, y)
        glEnd()

    def _render_locations(self):
        glColor4f(1, 1, 1, .01)
        glBegin(GL_LINES)
        for lx, ly in self._locations:
            x = lx * WORLD_WIDTH
            y = ly * WORLD_HEIGHT
            glVertex3f(x, BARS_BOTTOM, y)
            glVertex3f(x, BARS_TOP, y)
        glEnd()

    def _render_bar_grid_lines(self):
        glColor4f(1, 1, 1, 0.2)
        glBegin(GL_LINES)
        ny = 0
        for row in self._grid:
            y = (ny+0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    strength = pow(float(value) / self._location_max_value, 0.2)
                    x = (nx+0.5) / LOCATION_PRECISION * WORLD_WIDTH

                    # glColor4f(1, 1, 1, 0.5 * strength)
                    # glVertex3f(x, BARS_TOP, y)
                    # glColor4f(1, 1, 1, 0.05)
                    # glVertex3f(x, BARS_BOTTOM, y)

                    glVertex3f(x, BARS_TOP, y)
                    glVertex3f(x, BARS_TOP - strength*BARS_TOP, y)
                nx += 1
            ny += 1
        glEnd()

    def _render_bar_grid_points(self):
        self._render_grid_points(BARS_TOP)

    def _render_grid_points(self, h):
        glEnable(GL_POINT_SMOOTH)
        glPointSize(3.0)
        glBegin(GL_POINTS)
        glColor4f(1, 1, 1, 0.5)
        ny = 0
        for row in self._grid:
            y = (ny+0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    x = (nx+0.5) / LOCATION_PRECISION * WORLD_WIDTH
                    glVertex3f(x, h, y)
                nx += 1
            ny += 1
        glEnd()

    def _render_parabolae(self):
        ny = 0
        for row in self._grid:
            y = (ny+0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    x = (nx+0.5) / LOCATION_PRECISION * WORLD_WIDTH
                    strength = pow(float(value) / self._location_max_value, 0.4) * 0.5
                    glColor4f(1, 1, 1, strength)
                    self._render_parabola(x, y, self._here_x, self._here_y)
                nx += 1
            ny += 1

    def _render_grid_activity(self):
        glBegin(GL_LINES)
        ny = 0
        for row in self._grid_activity:
            y = (ny+0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    x = (nx+0.5) / LOCATION_PRECISION * WORLD_WIDTH
                    glColor4f(1, 1, 1, 1)
                    #self._render_parabola(x, y, self._here_x, self._here_y)
                    glVertex3f(x, BARS_TOP, y)
                    glColor4f(1, 1, 1, 0.25)
                    glVertex3f(x, BARS_BOTTOM, y)
                nx += 1
            ny += 1
        glEnd()

    def _render_parabola(self, x1, y1, x2, y2):
        h1 = 0
        h2 = 2
        glBegin(GL_LINE_STRIP)
        precision = 100
        for n in range(precision):
            r = float(n) / (precision - 1)
            x = x1 + (x2 - x1) * r
            y = y1 + (y2 - y1) * r
            h = h1 + (h2 - h1) * (math.cos((r - 0.5) / 5) - 0.995) / 0.005
            glVertex3f(x, h, y)
        glEnd()

    def _render_land_points(self):
        self._render_grid_points(0)

    def _render_active_traces(self):
        for segment in self.playing_segments.values():
            trace = self._traces[segment.peer.addr]
            self._render_trace(trace)

    def _render_trace(self, trace):
        glColor4f(1,1,1,1)
        glBegin(GL_LINE_STRIP)
        n = 1
        for lx, ly in trace:
            #opacity = float(n) / (len(trace)-1)
            #glColor4f(1,1,1, opacity)
            x = lx * WORLD_WIDTH
            y = ly * WORLD_HEIGHT
            glVertex3f(x, 0, y)
            n += 1
        glEnd()
Exemple #44
0
from gps import GPS
import time

g=GPS()

try:
	print("Valeur obtenue : {}".format(g.decimal_degrees("00133.5752")))
except ValueError as err:
	print(err)

time.sleep(10)
#!/usr/bin/python

import GeoIP
import random
from gps import GPS
import world

gi = GeoIP.open("GeoLiteCity.dat",GeoIP.GEOIP_STANDARD)

width = 1500.0
height = 1500.0

gps = GPS(width, height)

f = open('ips_on_world.svg', 'w')

def draw_path(points):
    x0, y0 = points[0]
    write_svg('<path style="stroke:%s;stroke-opacity=0.5;fill:none;" d="M%f,%f' % (
            "blue",
            x0, y0))
    for (x, y) in points[1:]:
        write_svg(' L%f,%f' % (x, y))
    write_svg('" />')

def write_svg(string):
    f.write(string)
    f.write('\n')

write_svg('<svg xmlns="http://www.w3.org/2000/svg" version="1.1">')
write_svg('<g>')
Exemple #46
0
 def compute_distance(self, location):
     current_location = self.gps.get_current_coordinates()
     return GPS.distance_between(current_location, location)
Exemple #47
0
 def __init__(self):
     self._geo_ip = GeoIP.open("%s/GeoLiteCity.dat" % os.path.dirname(__file__),
                               GeoIP.GEOIP_STANDARD)
     self._gps = GPS()
Exemple #48
0
#!/usr/bin/env python


import json
import time
import sys
from PIL import Image, ImageDraw
from map_plot import image_filename, c2s_x, c2s_y
from gps import GPS
gps = GPS()

CSIZE = 2
SEC = 60

img = Image.open('map_img/' + image_filename)
draw = ImageDraw.Draw(img)

t0 = 1435421469
t1 = 1435441950

for t in range(t0, t1, SEC):
    pos = gps.get(t)

    x = c2s_x(pos['long'])
    y = c2s_y(pos['lat'])

    draw.ellipse((x - CSIZE, y - CSIZE, x + CSIZE, y + CSIZE), outline=0)

img.show()

Exemple #49
0
class Geography(visualizer.Visualizer):
    def __init__(self, args):
        self._ip_locator = ip_locator.IpLocator()
        self._gps = GPS(WORLD_WIDTH, WORLD_HEIGHT)
        self._here_x = self._gps.x(HERE_LONGITUDE)
        self._here_y = self._gps.y(HERE_LATITUDE)
        self._load_locations()
        visualizer.Visualizer.__init__(self, args)
        self._set_camera_position(CAMERA_POSITION)
        self._set_camera_orientation(CAMERA_Y_ORIENTATION, CAMERA_X_ORIENTATION)
        self._world = world.World(WORLD_WIDTH, WORLD_HEIGHT)
        self.enable_3d()

    def _load_locations(self):
        self._locations = []
        self._grid = numpy.zeros((LOCATION_PRECISION, LOCATION_PRECISION), int)
        self._add_peers_from_log()
        self._location_max_value = numpy.max(self._grid)

    def _add_peers_from_log(self):
        for location in locations.get_locations():
            self._add_location(location)

    def _add_location(self, location):
        if location and location not in self._locations:
            self._locations.append(location)
            x, y = location
            nx = int(LOCATION_PRECISION * x)
            ny = int(LOCATION_PRECISION * y)
            self._grid[ny, nx] += 1
            return True

    def InitGL(self):
        visualizer.Visualizer.InitGL(self)
        glClearColor(0.0, 0.0, 0.0, 0.0)

    def render(self):
        glEnable(GL_LINE_SMOOTH)
        glHint(GL_LINE_SMOOTH_HINT, GL_NICEST)
        glEnable(GL_BLEND)
        glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)

        self._render_world()
        # self._render_bar_grid_lines()
        # self._render_bar_grid_points()
        self._render_parabolae()
        self._render_land_points()
        # self._render_locations()

    def _render_world(self):
        glColor3f(*LAND_COLOR)

        for path in self._world.paths:
            self._render_land(path)

    def _render_land(self, path):
        glBegin(GL_LINE_STRIP)
        for x, y in path:
            glVertex3f(x, 0, y)
        glEnd()

    def _render_locations(self):
        glColor4f(1, 1, 1, 0.01)
        glBegin(GL_LINES)
        for lx, ly in self._locations:
            x = lx * WORLD_WIDTH
            y = ly * WORLD_HEIGHT
            glVertex3f(x, BARS_BOTTOM, y)
            glVertex3f(x, BARS_TOP, y)
        glEnd()

    def _render_bar_grid_lines(self):
        glBegin(GL_LINES)
        ny = 0
        for row in self._grid:
            y = (ny + 0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    strength = pow(float(value) / self._location_max_value, 0.2)
                    x = (nx + 0.5) / LOCATION_PRECISION * WORLD_WIDTH

                    glColor4f(1, 1, 1, 0.5 * strength)
                    glVertex3f(x, BARS_TOP, y)
                    glColor4f(1, 1, 1, 0.05)
                    glVertex3f(x, BARS_BOTTOM, y)

                    # glColor4f(1, 1, 1, 0.2)
                    # glVertex3f(x, BARS_TOP, y)
                    # glVertex3f(x, BARS_TOP - strength*BARS_TOP, y)
                nx += 1
            ny += 1
        glEnd()

    def _render_bar_grid_points(self):
        self._render_grid_points(BARS_TOP)

    def _render_grid_points(self, h):
        glEnable(GL_POINT_SMOOTH)
        glPointSize(3.0)
        glBegin(GL_POINTS)
        glColor4f(1, 1, 1, 0.5)
        ny = 0
        for row in self._grid:
            y = (ny + 0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    x = (nx + 0.5) / LOCATION_PRECISION * WORLD_WIDTH
                    glVertex3f(x, h, y)
                nx += 1
            ny += 1
        glEnd()

    def _render_parabolae(self):
        ny = 0
        for row in self._grid:
            y = (ny + 0.5) / LOCATION_PRECISION * WORLD_HEIGHT
            nx = 0
            for value in row:
                if value > 0:
                    x = (nx + 0.5) / LOCATION_PRECISION * WORLD_WIDTH
                    strength = pow(float(value) / self._location_max_value, 0.4) * 0.8
                    glColor4f(1, 1, 1, strength)
                    self._render_parabola(x, y, self._here_x, self._here_y)
                nx += 1
            ny += 1

    def _render_parabola(self, x1, y1, x2, y2):
        h1 = 0
        h2 = 2
        glBegin(GL_LINE_STRIP)
        precision = 100
        for n in range(precision):
            r = float(n) / (precision - 1)
            x = x1 + (x2 - x1) * r
            y = y1 + (y2 - y1) * r
            h = h1 + (h2 - h1) * (math.cos((r - 0.5) / 5) - 0.995) / 0.005
            glVertex3f(x, h, y)
        glEnd()

    def _render_land_points(self):
        self._render_grid_points(BARS_BOTTOM)

    def _render_surface_points(self):
        self._render_grid_points(BARS_TOP)