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()