def gps_listener(self, data):
"""ROS callback for the gps topic"""
latitude = data.data[0] # In degrees
longitude = data.data[1]
# Converts lat long to x,y using FIXED origin
x, y = global_to_local(float(latitude), float(longitude))
#if abs(x) > OUTLIER_THRESHOLD or abs(y) > OUTLIER_THRESHOLD:
# return
self.gps_speed = data.data[8]
self.gps_yaw = np.deg2rad(data.data[7])
timestamp = data.data[0] / 1.e9
# Converts lat long to x,y using FIXED origin
if not self.ready:
# TODO: don't assume initial xdot and ydot are zero
self.k1_state = np.matrix([[x], [y], [0], [0]])
self.k2_state = np.matrix([[self.gps_yaw], [0]])
self.ready = True
self.last_timestamp = timestamp
delta_time = timestamp - self.last_timestamp
self.last_timestamp = timestamp
self.time_step = [data.data[10]]
# Converts lat long to x,y using FIXED origin
x, y = global_to_local(float(latitude), float(longitude))
if abs(x) > OUTLIER_THRESHOLD or abs(y) > OUTLIER_THRESHOLD:
return
self.gps_speed = data.data[8]
self.gps_yaw = np.deg2rad(data.data[7])
timestamp = data.data[0] / 1.e9
# Converts lat long to x,y using FIXED origin
dim = [MultiArrayDimension('data', 1, 2)]
layout = MultiArrayLayout(dim, 0)
self.pub_debug.publish(layout, gps_debug_state)
gps_sensor_data = GPSData(x=x,
y=y,
yaw=self.gps_yaw,
speed=self.gps_speed,
timestamp=None)
self.position_filter.update(delta_time, gps_sensor=gps_sensor_data)
def kalman_compare_retro(gps_filename, bike_filename):
gps_data = []
with open(gps_filename) as gps_file:
gps_reader = list(csv.reader(gps_file, delimiter=","))
for row in gps_reader[1:]:
# field0 is lat, field1 is long, field7 is yaw in degrees,
# field8 is speed from the gps (meters per second)
# field10 is timestep (miliseconds)
x, y = global_to_local(float(row[2]), float(row[3]))
if -0.0001 < x < 0.0001:
continue
# print '{:6.3f} {:6.3f}'.format(x, y)
gps_data.append([x, y, float(row[9])*np.pi/180, float(row[10]), float(row[12])])
# The Kalman filter wants the GPS data in matrix form
gps_matrix = np.matrix(gps_data)
# Run the Kalman filter
output_old = kalman.kalman_retro_old(gps_matrix)
# Run updated Kalman filter
sensors = SensorData(gps_filename=gps_filename, bike_filename=bike_filename)
output_new = kalman.kalman_retro(sensors)
# Plot the GPS data
plt.scatter(sensors.gps.x, sensors.gps.y, c='r')
# Plot the old and new filter output
plt.scatter([output_old[:,0]], [output_old[:,1]], c='b')
plt.scatter(output_new['x'], output_new['y'], c='g')
# Show everything
plt.show()
def loc_listener_old(self, data):
#if hasattr(self, 'path_patch') and self.path_patch:
# self.path_patch.remove()
#if len(self.bike_trajectory):
# self.path_patch = PathPatch(Path(self.bike_trajectory), fill=False,
# linewidth=2)
# axes.add_patch(self.path_patch)
# Plot the bike as a wedge pointing in the direction bike.psi
bike_x, bike_y = global_to_local(data.lat, data.lng)
#rospy.logerr("%f,%f"%(bike_x,bike_y))
self.circle.center = (bike_x,bike_y)
#if self.prev_bike_patch: self.prev_bike_patch.remove()
#wedge_angle = 45 # The angle covered by the wedge
#bike_heading = data.yaw
#bike_polygon = Wedge((bike_x, bike_y), 1,
# bike_heading - wedge_angle / 2 + 180,
# bike_heading + wedge_angle / 2 + 180, fc="black")
#rospy.logerr(dir(bike_polygon))
#self.axes.add_patch(bike_polygon)
#self.prev_bike_patch = bike_polygon
#plt.show(block=True)
#plt.pause(0.00000000000001)
self.bike_trajectory.append((bike_x, bike_y))
plt.draw()
def make_location_message(self, lat, lng):
x, y = global_to_local(lat, lng)
if False: #(y - self.last_y) == 0 and (x - self.last_x) == 0:
# we haven't moved
return None
yaw = math.atan2(y - self.last_y, x - self.last_x)
speed = math.sqrt((y - self.last_y)**2 + (x - self.last_x)**2)
self.last_x, self.last_y = x, y
return Location(lat, lng, yaw, speed)
def load_gps_state(self, gps_filename):
x_data = []
y_data = []
yaw_data = []
speed_data = []
timestamp_data = []
with open(gps_filename) as gps_file:
csv_reader = list(csv.reader(gps_file, delimiter=','))[1:]
for i, row in enumerate(csv_reader):
x, y = global_to_local(float(row[2]), float(row[3]))
yaw = align_radians(-float(row[9]) * np.pi / 180. + np.pi / 2)
speed = float(row[10])
timestamp = float(row[0]) / 1.e9
# Remove outliers.
if (np.linalg.norm([x, y]) > GPS_RANGE_THRESHOLD
or speed > GPS_SPEED_THRESHOLD):
print('ignoring outlier gps[{}] = ({}, {})'.format(
i, x, y))
continue
x_data.append(x)
y_data.append(y)
yaw_data.append(yaw)
speed_data.append(speed)
timestamp_data.append(timestamp)
# Shift timestamps to get time relative to start.
start_time = min(timestamp_data)
timestamp_data = [t - start_time for t in timestamp_data]
return GPSData(x=np.array(x_data),
y=np.array(y_data),
yaw=np.array(yaw_data),
speed=np.array(speed_data),
timestamp=np.array(timestamp_data))
def main(self):
rospy.init_node("plotter")
while not rospy.has_param('path'):
time.sleep(1)
self.path = rospy.get_param("path")
plt.ion() # enables interactive plotting
waypoints = map(lambda p: global_to_local(*p), self.path)
rospy.logerr(waypoints)
map_model = mapModel.Map_Model(None, waypoints, [], [])
paths = map_model.paths
fig = plt.figure()
#ax = plt.axes(**find_display_bounds(map_model.waypoints))
ax = plt.axes(xlim=(-100,100), ylim=(-100,100))
lc = mc.LineCollection(paths, linewidths=2, color = "blue")
ax.add_collection(lc)
# For plotting the bicycle
self.axes = plt.gca()
self.circle = Circle((0,0),1,ec='r', fc='none')
self.axes.add_patch(self.circle)
self.circle2 = Circle((0,0),1,ec='b', fc='none')
self.axes.add_patch(self.circle2)
# Holds past locations of the bike, for plotting
self.bike_trajectory = []
# We need to keep this around to clear it after path updates
#self.path_patch = None
self.prev_bike_patch = None
rospy.Subscriber("/android/gps/fused", Location, self.loc_listener) # RED
rospy.Subscriber("/android/gps/gps", Location, self.loc2_listener) # BLUE
plt.show(block=True)
def loc2_listener(self, data):
self.circle2.center = global_to_local(data.lat, data.lng)
plt.draw()
def update_from_android_gps(data):
new_bike.xB, new_bike.yB = global_to_local(data.lat, data.lng)
new_bike.psi, new_bike.v = data.yaw, data.speed
if __name__ == '__main__':
try:
old_psi = 0
old_v = 0
d_psi = 0
new_bike = bikeState.Bike(0, -10, 0.1, np.pi / 3, 0, 0, 3.00)
# right intersection to middle intersection
orig_waypoints = [(42.444253, -76.483271), (42.444238, -76.483661)]
#straight path bottom of stairs to middle intersection
#orig_waypoints = [(133.74892645377858, -432.5469806370678), (114.05523219700194, -395.85300512410294)]
#Straight forward path
#orig_waypoints = [(42.444260000000001, -76.48366), (42.44447, -76.48367), (42.44485, -76.48369000000001)]
#long path
#orig_waypoints = [(42.44422, -76.43866), (42,44447, -76.48367), (42.44458, -76.4835)]
#orig_waypoints = [(164.92918389320673, -412.53122538008699), (160.82636519097008, -408.08352424480717), (158.36456020192119, -400.29993577850547), (157.54391784874556, -395.85215731941992), (152.62045116162616, -385.84472352909091), (141.95309049412472, -369.16571007809335), (133.74760279568892, -363.6061261255179), (124.72163122543957, -360.27044577941609), (118.1572971243023, -358.04666168562972), (110.7724130084174, -354.71093523541589), (97.643830726023069, -354.7111316365864), (98.464555727252943, -368.05451128333181), (101.74674006642893, -370.2783626484474), (102.56727829387685, -370.27835061499496), (103.3878330236748, -371.39028775136137), (114.05523219700194, -395.85300512410294), (134.56949349422308, -433.6589141004269)]
#converting from lat long to local coordinate plane
waypoints = [0 for _ in range(len(orig_waypoints))]
for i in range(len(orig_waypoints)):
latitude, longitude = orig_waypoints[i][0], orig_waypoints[i][1]
waypoints[i] = global_to_local(float(latitude), float(longitude))
new_map = mapModel.Map_Model(new_bike, waypoints, [], [])
new_nav = nav.Nav(new_map)
talker()
except rospy.ROSInterruptException:
rospy.logerr('this should never print')
pass