def connect(self):
if self.session:
return
try:
self.session = GPS(mode=WATCH_ENABLE)
except OSError:
return
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')
def __init__(self):
super(EventProcessor, self).__init__()
self.message_handler = MessageHandler()
self.gps = GPS()
Message.init()
Detector.start_plugins()
Reactor.add_plugin_events()
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
def start():
# 启动GPS
GPS_thread = GPS()
GPS_thread.start()
# 启动提示预警
remind_thread = remind.remind_thread()
remind_thread.start()
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 )
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)
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()
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
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)
def __init__(self):
super(EventProcessor, self).__init__()
self.message_handler = MessageHandler()
self.gps = GPS()
Message.init()
Detector.start_plugins()
Reactor.add_plugin_events()
def connect(self):
if self.session:
return
try:
self.session = GPS(mode=WATCH_ENABLE)
except OSError:
return
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)
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
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 __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()
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)
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"
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
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
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)
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)
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))
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))
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)
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)
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
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()
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()
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()
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")
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 )
def testCarWorks(self):
gps = GPS(state=['son-at-home', 'car-works'],
goals=['son-at-school'],
ops=self.school_ops)
self.assertEqual(gps.solve(), 'solved')
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')
def compute_distance(self, location):
current_location = self.gps.get_current_coordinates()
return GPS.distance_between(current_location, location)
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()
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>')
def compute_distance(self, location):
current_location = self.gps.get_current_coordinates()
return GPS.distance_between(current_location, location)
def __init__(self):
self._geo_ip = GeoIP.open("%s/GeoLiteCity.dat" % os.path.dirname(__file__),
GeoIP.GEOIP_STANDARD)
self._gps = GPS()
#!/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()
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)