def project(lat, lon, lat_orig, lon_orig): RADIUS = 6371 * 1000 lon_per_deg = RADIUS * 2 * math.pi / 360 lat_per_deg = lon_per_deg * math.cos(math.radians(lat_orig)) x = (lon - lon_orig) * lon_per_deg y = (lat - lat_orig) * lat_per_deg return (x, y) try: while 1: try: correction = rtcm_sub.recv() print("got correction") except: pass else: ser.write(correction["rtcm"]) line = ser.readline() #print line if line[0:2] == '$G': msg = pynmea2.parse(str(line)) if (msg.sentence_type == "GGA"): print(msg.latitude, msg.longitude) print(repr(msg)) #print(msg.timestamp)
class Rover: def __init__(self): self.imu = Subscriber(8220, timeout=2) self.gps = Subscriber(8280, timeout=3) self.auto_control = Subscriber(8310, timeout=5) self.lights = Publisher(8311) self.cmd_vel = Publisher(8830) self.logger = Logger(8312) self.aruco = Subscriber(8390, timeout=3) time.sleep(2) # delay to give extra time for gps message self.start_gps = self.gps.recv() self.cmd_sent = False def project(self, lat, lon): lat_orig = self.start_gps['lat'] lon_orig = self.start_gps['lon'] RADIUS = 6371 * 1000 lon_per_deg = RADIUS * 2 * math.pi / 360 lat_per_deg = lon_per_deg * math.cos(math.radians(lat_orig)) x = (lon - lon_orig) * lon_per_deg y = (lat - lat_orig) * lat_per_deg return (x, y) def get_pose(self): gps = self.gps.get() # heading = math.radians( 360 -self.imu.get()['angle'][0] ) heading = math.radians(self.imu.get()['angle'][0]) return Pose(*self.project(gps['lat'], gps['lon']), heading) def send_vel(self, forward, twist): msg = {"f": forward, "t": twist} if self.cmd_sent: print("ERROR Multiple commands being sent!") self.cmd_vel.send(msg) self.cmd_sent = True print(msg) def get_aruco(self): markers = self.aruco.get() out = {} for marker in markers: idx, dist, angle = marker['id'], marker['dist'], marker['angle'] out[idx] = Bearing(dist, math.radians(angle)) return out def get_cmd(self): return self.auto_control.get() def get_waypoints(self) -> List["Pose"]: wps = self.auto_control.get()['waypoints'] out = [] for waypoint in wps: out.append(Pose(*self.project(waypoint['lat'], waypoint['lon']))) return out def next_iteration(self): self.cmd_sent = False self.logger.next()