def __init__(self, *args, **kwargs):
"""Stores user values"""
super(Anim_Attacker, self).__init__(*args, **kwargs)
self.horns = Polygon(0 * self.get_horn_array(),
fc=self.og_face_color,
ec="k")
# .4 if self.high_res else .5
self.horns.set_linewidth(.5)
class Funnel(OriginalFunnel):
"""Defines a funnel (an angular region defined by three points) in R^2.
This funnel is drawable.
"""
def __init__(self, cusp: Point, first: Point, second: Point, *args, **kwargs):
"""Initialize a new funnel given by three points."""
super(Funnel, self).__init__(cusp, first, second)
if 'color' not in kwargs:
kwargs['color'] = 'red'
if 'fill' not in kwargs:
kwargs['fill'] = None
kwargs['closed'] = False
if 'zorder' not in kwargs:
kwargs['zorder'] = 10
self.polygon = Polygon([
(first + (first - cusp) * 10).tuple(),
cusp.tuple(),
(second + (second - cusp) * 10).tuple()
], *args, **kwargs)
gca().add_patch(self.polygon)
draw()
@OriginalFunnel.first.setter
def first(self, value: Point) -> None:
"""Set the first boundary point."""
super(Funnel, self.__class__).first.__set__(self, value)
self._update_polygon()
@OriginalFunnel.second.setter
def second(self, value: Point) -> None:
"""Set the second boundary point."""
super(Funnel, self.__class__).second.__set__(self, value)
self._update_polygon()
@OriginalFunnel.cusp.setter
def cusp(self, value: Point) -> None:
"""Set the second boundary point."""
super(Funnel, self.__class__).cusp.__set__(self, value)
self._update_polygon()
def _update_polygon(self) -> None:
xy = [
(self._first + (self._first - self._cusp) * 10).tuple(),
self._cusp.tuple(),
(self._second + (self._second - self._cusp) * 10).tuple()
]
self.polygon.set_xy(xy)
draw()
def draw(self, axes: Axes) -> None:
"""Add the drawable polygon to the given matplotlib object."""
axes.add_patch(self.polygon)
def createHorizonPolygons(self):
"""Creates the two polygons to show the sky and ground."""
# Sky Polygon
vertsTop = [[-1, 0], [-1, 1], [1, 1], [1, 0], [-1, 0]]
self.topPolygon = Polygon(vertsTop, facecolor="dodgerblue", edgecolor="none")
self.axes.add_patch(self.topPolygon)
# Ground Polygon
vertsBot = [[-1, 0], [-1, -1], [1, -1], [1, 0], [-1, 0]]
self.botPolygon = Polygon(vertsBot, facecolor="brown", edgecolor="none")
self.axes.add_patch(self.botPolygon)
def createHorizonPolygons(self):
'''Creates the two polygons to show the sky and ground.'''
# Sky Polygon
vertsTop = [[-1,0],[-1,1],[1,1],[1,0],[-1,0]]
self.topPolygon = Polygon(vertsTop,facecolor='dodgerblue',edgecolor='none')
self.axes.add_patch(self.topPolygon)
# Ground Polygon
vertsBot = [[-1,0],[-1,-1],[1,-1],[1,0],[-1,0]]
self.botPolygon = Polygon(vertsBot,facecolor='brown',edgecolor='none')
self.axes.add_patch(self.botPolygon)
def start(self, **kwargs):
# create the plot
super().reset()
if self.sim.graphics:
self.fig = self.sim.create_figure()
self.ax = self.fig.gca()
if self.square:
self.ax.set_aspect('equal')
args = []
kwargs = {}
if self.path:
style = self.pathstyle
if isinstance(style, dict):
kwargs = style
elif isinstance(style, str):
args = [style]
self.line, = self.ax.plot(self.xdata, self.ydata, *args,
**kwargs)
poly = Polygon(self.vertices,
closed=True,
edgecolor=self.color,
facecolor=self.fill)
self.vehicle = self.ax.add_patch(poly)
self.ax.grid(True)
self.ax.set_xlabel(self.labels[0])
self.ax.set_ylabel(self.labels[1])
self.ax.set_title(self.name)
if self.scale != 'auto':
self.ax.set_xlim(*self.scale[0:2])
self.ax.set_ylim(*self.scale[2:4])
if self.init is not None:
self.init(self.ax)
def get_triangle_angles(triangle: plt.Polygon) -> tuple:
points = triangle.get_xy()
a, b, c = points[:-1]
abc = rustic.get_angle(a, b, c)
bac = rustic.get_angle(b, a, c)
bca = rustic.get_angle(b, c, a)
return abc, bac, bca
def plot_simplices(axis, R, F):
for s in F:
x = R.data[list(s)]
if s.dimension() == 0:
axis.plot(x[:, 0], x[:, 1], 'o', c='black', zorder=2)
elif s.dimension() == 1:
axis.plot(x[:, 0], x[:, 1], c='red', alpha=0.7, zorder=1)
elif s.dimension() == 2:
axis.add_patch(Polygon(x, color='black', alpha=0.1, zorder=0))
def plot_results(ages, subsidence, thickness_list, sea_levels, bathymetries):
""" Plots tectonic subsidence and sediment thickness change over time. """
# plot setup
fig = plt.figure(figsize=(12, 9), facecolor='white')
axes = plt.gca()
plt.grid()
axes.invert_xaxis()
axes.set_xlabel("Time [Ma]", labelpad=15)
axes.set_ylabel("Depth [m]", labelpad=15)
axes.tick_params(axis='both',
which='major',
pad=10,
direction='out',
size=5)
axes.xaxis.label.set_fontsize(18)
axes.yaxis.label.set_fontsize(18)
for item in axes.get_xticklabels() + axes.get_yticklabels():
item.set_fontsize(14)
# actual data
subs, = plt.plot(ages, [0] + subsidence,
'--',
color='#0077B8',
lw=5,
label='Subsidence')
plt.legend(handles=[subs], loc=3)
horizon_offset = [
w - bathymetries[0] - (s - sea_levels[0])
for w, s in zip(bathymetries, sea_levels)
]
horizons = [
list(accumulate(chain([ho], t)))
for ho, t in zip(horizon_offset, thickness_list)
]
axes.set_xlim([max(ages), min(ages)])
axes.set_ylim([max(max(h) for h in horizons), min(horizon_offset)])
patches = []
n_patches = len(ages) - 1
x_indices = [n_patches]
for i, j in enumerate(reversed(range(n_patches))):
x_indices = [j] + x_indices + [j + 1]
y_indices = list(range(i + 1)) + list(range(i + 1, -1, -1))
points = [[ages[x_id], horizons[x_id - 1][y_id]]
for x_id, y_id in zip(x_indices, y_indices)]
patches.append(Polygon(points))
# we have to fix the last polygon
xy = patches[-1].get_xy()
xy[0, 1] = xy[-1, 1] = 0.0
patches[-1].set_xy(xy)
p = PatchCollection(patches, cmap='terrain', alpha=0.7)
p.set_array(np.arange(n_patches))
axes.add_collection(p)
plt.show()
def __init__(self, cusp: Point, first: Point, second: Point, *args, **kwargs):
"""Initialize a new funnel given by three points."""
super(Funnel, self).__init__(cusp, first, second)
if 'color' not in kwargs:
kwargs['color'] = 'red'
if 'fill' not in kwargs:
kwargs['fill'] = None
kwargs['closed'] = False
if 'zorder' not in kwargs:
kwargs['zorder'] = 10
self.polygon = Polygon([
(first + (first - cusp) * 10).tuple(),
cusp.tuple(),
(second + (second - cusp) * 10).tuple()
], *args, **kwargs)
gca().add_patch(self.polygon)
draw()
class Anim_Attacker(Anim_User):
"""Animated User"""
og_face_color = "r"
def __init__(self, *args, **kwargs):
"""Stores user values"""
super(Anim_Attacker, self).__init__(*args, **kwargs)
self.horns = Polygon(0 * self.get_horn_array(),
fc=self.og_face_color,
ec="k")
# .4 if self.high_res else .5
self.horns.set_linewidth(.5)
def get_horn_array(self):
return np.array([
self.patch.center,
[
self.patch.center[0] - Anim_User.patch_radius,
self.patch.center[1]
],
[
self.patch.center[0] - Anim_User.patch_radius,
self.patch.center[1] + Anim_User.patch_radius
], self.patch.center,
[
self.patch.center[0] + Anim_User.patch_radius,
self.patch.center[1]
],
[
self.patch.center[0] + Anim_User.patch_radius,
self.patch.center[1] + Anim_User.patch_radius
], self.patch.center
])
def add_to_anim(self, ax, zorder):
"""Adds patches to plot"""
self.horns.set_zorder(zorder)
ax.add_patch(self.horns)
self.horns.set_xy(self.get_horn_array())
return super(Anim_Attacker, self).add_to_anim(ax, zorder + 1)
def _move_user(self, *args, **kwargs):
super(Anim_Attacker, self)._move_user(*args, **kwargs)
self.horns.set_xy(self.get_horn_array())
@property
def anim_objects(self):
"""Animation objects used by the animation"""
return [self.horns] + super(Anim_Attacker, self).anim_objects
def plot_triangle(self, axis, s, shadow=True, **kw):
p = self.data[list(s)]
if 'c' in kw:
kw['color'] = kw['c']
del kw['c']
kw['color'] = 'black' if not 'color' in kw else kw['color']
kw['alpha'] = 0.1 if not 'alpha' in kw else kw['alpha']
kw['zorder'] = 0 if not 'zorder' in kw else kw['zorder']
if shadow:
kw['path_effects'] = [pfx.withSimplePatchShadow()]
return axis.add_patch(Polygon(p, **kw))
def __init__(self, ax, shapetype):
self.diamond_coords = np.array([0, 0])
if shapetype == "patch":
self.patch = Polygon(np.array([[0,0], [0,1], [1,1]]), fill=True, closed=True)
ax.add_patch(self.patch)
self.outline = Polygon(np.array([[0,0], [0,1], [1,1]]), fill=False, closed=True)
ax.add_patch(self.outline)
self.line = None
elif shapetype == "line":
self.patch = None
self.outline = None
self.line = Polygon(np.array([[0,0], [0,1], [1,1]]), fill=False, closed=False)
ax.add_patch(self.line)
else:
raise Exception(shapetype, "not a recognized shapetype")
diamond_size = get_diamond_size(ax)
if diamond_size is not None:
self.diamond_contour = np.array([[1, 0], [0, -1], [-1, 0], [0, 1]]) * diamond_size
self.diamond = Polygon(self.diamond_contour, fill=True, closed=True)
ax.add_patch(self.diamond)
else:
self.diamond_contour = None
self.diamond = None
for attr_name in format_arg_dict.keys():
_attr = getattr(self, attr_name)
if _attr is None:
continue
if "line" in attr_name:
set_line_style(_attr, **my_default_colour_etc_settings[attr_name])
else:
set_patch_style(_attr, **my_default_colour_etc_settings[attr_name])
self.clip_patches = []
self.move_history = {'flip' : False, 'stretch_x' : 1., 'stretch_y' : 1., 'turn' : 0}
self.shape_kwargs = {}
self.shapename = None
Shape.all_shapes.append(self)
def draw_source(self, ax):
source = self
neg = np.asarray([1, -1])
area = Polygon(
(source.start, source.end, source.end * neg, source.start * neg),
color=[0.4, 0.4, 0.4, 0.25],
zorder=0)
ax.add_artist(area)
upper_line = Line2D(source.x_span, source.radius, color='r')
lower_line = Line2D(source.x_span, -source.radius, color='r')
ax.add_artist(upper_line)
ax.add_artist(lower_line)
def clone_patch(init_patch):
if init_patch is None:
return None
else:
result = Polygon(xy=dummy_xy,
closed=init_patch.get_closed())
result.update_from(other=init_patch)
result.set_xy(np.copy(init_patch.get_xy()))
init_patch.axes.add_patch(result)
assert is_the_same_contour(p1=result.get_xy(),
p2=init_patch.get_xy())
return result
def plot_poly(contours, ax, coord_fn, color):
"""Plots a polygon where contours is a list of M polygons. Each polygon is an Nx2 array of
its vertices. Coord_fn is meant to be the coordinate function of a georaster image."""
p = []
for i, poly in enumerate(contours):
# Avoid degenerate polygons
if len(poly) < 3:
continue
pts = np.array(poly).squeeze()
try:
xs, ys = coord_fn(Xpixels=list(pts[:, 0]), Ypixels=list(pts[:, 1]))
except IndexError as e:
print("error on translating poly {}".format(i))
p.append(Polygon(np.vstack([xs, ys]).T, facecolor='red'))
col = PatchCollection(p)
col.set_color(color)
ax.add_collection(col)
return ax
def clip(self, clip_outline):
for what in [self.patch, self.line, self.outline]:
if what is None:
continue
clip_patch = None
if isinstance(clip_outline, Shape):
clip_xy = clip_outline.get_xy()
for clip_candidate in [clip_outline.patch, clip_outline.line]:
if clip_candidate is not None:
if clip_candidate.get_zorder() == what.get_zorder():
clip_patch = clip_candidate
else:
clip_xy = Shape._get_xy(clip_outline)
if clip_patch is None:
clip_patch = Polygon(clip_xy,
fc='none',
ec='none',
zorder=what.get_zorder())
what.axes.add_patch(clip_patch)
self.clip_patches.append(clip_patch)
what.set_clip_path(clip_patch)
def draw_source(self, ax):
source = self
c1 = np.asarray(source.get_loop_list())
c2 = c1 * [1, -1, 1]
try:
dia = np.sqrt(
np.power(c1[0][0] - c1[1][0], 2) +
np.power(c1[0][1] - c1[1][1], 2))
dia = min(dia, 1.0)
except IndexError:
dia = 1.0
neg = np.asarray([1, -1])
poly = Polygon(
(source.start, source.end, source.end * neg, source.start * neg),
color=[0.4, 0.4, 0.4, 0.25],
zorder=0)
ax.add_artist(poly)
for loop in c1:
circle = Circle((loop[0], loop[1]), dia / 2, color='r', fill=None)
ax.add_artist(circle)
for loop in c2:
circle = Circle((loop[0], loop[1]), dia / 2, color='r', fill=None)
ax.add_artist(circle)
def add_polygon_2d(self,
points=[],
poly_id="undefined",
ax=None,
**kwargs):
"""Add a polygon specified by list of input points
:param list points: list with :class:`GeoPoint` objects
:param str poly_id: string ID of this object (e.g. for
deletion, default: "undefined")
"""
if ax is None:
ax = self.ax
if not "label" in kwargs:
kwargs["label"] = poly_id
coords = []
for p in points:
try:
coords.append(self(p.longitude, p.latitude))
except Exception as e:
print("Failed to add one point to poly: " + repr(e))
polygon = Polygon(coords, **kwargs)
ax.add_patch(polygon)
class HorizonFrame(wx.Frame):
""" The main frame of the horizon indicator."""
def __init__(self, state, title):
self.state = state
# Create Frame and Panel(s)
wx.Frame.__init__(self, None, title=title)
state.frame = self
# Initialisation
self.initData()
self.initUI()
self.startTime = time.time()
def initData(self):
# Initialise Attitude
self.pitch = 0.0 # Degrees
self.roll = 0.0 # Degrees
self.yaw = 0.0 # Degrees
# History Values
self.oldRoll = 0.0 # Degrees
# Initialise Rate Information
self.airspeed = 0.0 # m/s
self.relAlt = 0.0 # m relative to home position
self.climbRate = 0.0 # m/s
self.altHist = [] # Altitude History
self.timeHist = [] # Time History
self.altMax = 0.0 # Maximum altitude since startup
# Initialise HUD Info
self.heading = 0.0 # 0-360
# Initialise Battery Info
self.voltage = 0.0
self.current = 0.0
self.batRemain = 0.0
# Initialise Mode and State
self.mode = "UNKNOWN"
self.armed = ""
self.safetySwitch = ""
# Intialise Waypoint Information
self.currentWP = 0
self.finalWP = 0
self.wpDist = 0
self.nextWPTime = 0
self.wpBearing = 0
def initUI(self):
# Create Event Timer and Bindings
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
self.timer.Start(100)
self.Bind(wx.EVT_IDLE, self.on_idle)
self.Bind(wx.EVT_CHAR_HOOK, self.on_KeyPress)
# Create Panel
self.panel = wx.Panel(self)
self.vertSize = 0.09
self.resized = False
# Create Matplotlib Panel
self.createPlotPanel()
# Fix Axes - vertical is of length 2, horizontal keeps the same lengthscale
self.rescaleX()
self.calcFontScaling()
# Create Horizon Polygons
self.createHorizonPolygons()
# Center Pointer Marker
self.thick = 0.015
self.createCenterPointMarker()
# Pitch Markers
self.dist10deg = 0.2 # Graph distance per 10 deg
self.createPitchMarkers()
# Add Roll, Pitch, Yaw Text
self.createRPYText()
# Add Airspeed, Altitude, Climb Rate Text
self.createAARText()
# Create Heading Pointer
self.createHeadingPointer()
# Create North Pointer
self.createNorthPointer()
# Create Battery Bar
self.batWidth = 0.1
self.batHeight = 0.2
self.rOffset = 0.35
self.createBatteryBar()
# Create Mode & State Text
self.createStateText()
# Create Waypoint Text
self.createWPText()
# Create Waypoint Pointer
self.createWPPointer()
# Create Altitude History Plot
self.createAltHistoryPlot()
# Show Frame
self.Show(True)
self.pending = []
def createPlotPanel(self):
"""Creates the figure and axes for the plotting panel."""
self.figure = Figure()
self.axes = self.figure.add_subplot(111)
self.canvas = FigureCanvas(self, -1, self.figure)
self.canvas.SetSize(wx.Size(300, 300))
self.axes.axis("off")
self.figure.subplots_adjust(left=0, right=1, top=1, bottom=0)
self.sizer = wx.BoxSizer(wx.VERTICAL)
self.sizer.Add(self.canvas, 1, wx.EXPAND, wx.ALL)
self.SetSizerAndFit(self.sizer)
self.Fit()
def rescaleX(self):
"""Rescales the horizontal axes to make the lengthscales equal."""
self.ratio = self.figure.get_size_inches()[0] / float(self.figure.get_size_inches()[1])
self.axes.set_xlim(-self.ratio, self.ratio)
self.axes.set_ylim(-1, 1)
def calcFontScaling(self):
"""Calculates the current font size and left position for the current window."""
self.ypx = self.figure.get_size_inches()[1] * self.figure.dpi
self.xpx = self.figure.get_size_inches()[0] * self.figure.dpi
self.fontSize = self.vertSize * (self.ypx / 2.0)
self.leftPos = self.axes.get_xlim()[0]
self.rightPos = self.axes.get_xlim()[1]
def checkReszie(self):
"""Checks if the window was resized."""
if not self.resized:
oldypx = self.ypx
oldxpx = self.xpx
self.ypx = self.figure.get_size_inches()[1] * self.figure.dpi
self.xpx = self.figure.get_size_inches()[0] * self.figure.dpi
if (oldypx != self.ypx) or (oldxpx != self.xpx):
self.resized = True
else:
self.resized = False
def createHeadingPointer(self):
"""Creates the pointer for the current heading."""
self.headingTri = patches.RegularPolygon((0.0, 0.80), 3, 0.05, color="k", zorder=4)
self.axes.add_patch(self.headingTri)
self.headingText = self.axes.text(
0.0,
0.675,
"0",
color="k",
size=self.fontSize,
horizontalalignment="center",
verticalalignment="center",
zorder=4,
)
def adjustHeadingPointer(self):
"""Adjust the value of the heading pointer."""
self.headingText.set_text(str(self.heading))
self.headingText.set_size(self.fontSize)
def createNorthPointer(self):
"""Creates the north pointer relative to current heading."""
self.headingNorthTri = patches.RegularPolygon((0.0, 0.80), 3, 0.05, color="k", zorder=4)
self.axes.add_patch(self.headingNorthTri)
self.headingNorthText = self.axes.text(
0.0,
0.675,
"N",
color="k",
size=self.fontSize,
horizontalalignment="center",
verticalalignment="center",
zorder=4,
)
def adjustNorthPointer(self):
"""Adjust the position and orientation of
the north pointer."""
self.headingNorthText.set_size(self.fontSize)
headingRotate = mpl.transforms.Affine2D().rotate_deg_around(0.0, 0.0, self.heading) + self.axes.transData
self.headingNorthText.set_transform(headingRotate)
if (self.heading > 90) and (self.heading < 270):
headRot = self.heading - 180
else:
headRot = self.heading
self.headingNorthText.set_rotation(headRot)
self.headingNorthTri.set_transform(headingRotate)
# Adjust if overlapping with heading pointer
if (self.heading <= 10.0) or (self.heading >= 350.0):
self.headingNorthText.set_text("")
else:
self.headingNorthText.set_text("N")
def toggleWidgets(self, widgets):
"""Hides/shows the given widgets."""
for wig in widgets:
if wig.get_visible():
wig.set_visible(False)
else:
wig.set_visible(True)
def createRPYText(self):
"""Creates the text for roll, pitch and yaw."""
self.rollText = self.axes.text(
self.leftPos + (self.vertSize / 10.0),
-0.97 + (2 * self.vertSize) - (self.vertSize / 10.0),
"Roll: %.2f" % self.roll,
color="w",
size=self.fontSize,
)
self.pitchText = self.axes.text(
self.leftPos + (self.vertSize / 10.0),
-0.97 + self.vertSize - (0.5 * self.vertSize / 10.0),
"Pitch: %.2f" % self.pitch,
color="w",
size=self.fontSize,
)
self.yawText = self.axes.text(
self.leftPos + (self.vertSize / 10.0), -0.97, "Yaw: %.2f" % self.yaw, color="w", size=self.fontSize
)
self.rollText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.pitchText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.yawText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
def updateRPYLocations(self):
"""Update the locations of roll, pitch, yaw text."""
# Locations
self.rollText.set_position(
(self.leftPos + (self.vertSize / 10.0), -0.97 + (2 * self.vertSize) - (self.vertSize / 10.0))
)
self.pitchText.set_position(
(self.leftPos + (self.vertSize / 10.0), -0.97 + self.vertSize - (0.5 * self.vertSize / 10.0))
)
self.yawText.set_position((self.leftPos + (self.vertSize / 10.0), -0.97))
# Font Size
self.rollText.set_size(self.fontSize)
self.pitchText.set_size(self.fontSize)
self.yawText.set_size(self.fontSize)
def updateRPYText(self):
"Updates the displayed Roll, Pitch, Yaw Text"
self.rollText.set_text("Roll: %.2f" % self.roll)
self.pitchText.set_text("Pitch: %.2f" % self.pitch)
self.yawText.set_text("Yaw: %.2f" % self.yaw)
def createCenterPointMarker(self):
"""Creates the center pointer in the middle of the screen."""
self.axes.add_patch(
patches.Rectangle((-0.75, -self.thick), 0.5, 2.0 * self.thick, facecolor="orange", zorder=3)
)
self.axes.add_patch(patches.Rectangle((0.25, -self.thick), 0.5, 2.0 * self.thick, facecolor="orange", zorder=3))
self.axes.add_patch(patches.Circle((0, 0), radius=self.thick, facecolor="orange", edgecolor="none", zorder=3))
def createHorizonPolygons(self):
"""Creates the two polygons to show the sky and ground."""
# Sky Polygon
vertsTop = [[-1, 0], [-1, 1], [1, 1], [1, 0], [-1, 0]]
self.topPolygon = Polygon(vertsTop, facecolor="dodgerblue", edgecolor="none")
self.axes.add_patch(self.topPolygon)
# Ground Polygon
vertsBot = [[-1, 0], [-1, -1], [1, -1], [1, 0], [-1, 0]]
self.botPolygon = Polygon(vertsBot, facecolor="brown", edgecolor="none")
self.axes.add_patch(self.botPolygon)
def calcHorizonPoints(self):
"""Updates the verticies of the patches for the ground and sky."""
ydiff = math.tan(math.radians(-self.roll)) * float(self.ratio)
pitchdiff = self.dist10deg * (self.pitch / 10.0)
# Sky Polygon
vertsTop = [
(-self.ratio, ydiff - pitchdiff),
(-self.ratio, 1),
(self.ratio, 1),
(self.ratio, -ydiff - pitchdiff),
(-self.ratio, ydiff - pitchdiff),
]
self.topPolygon.set_xy(vertsTop)
# Ground Polygon
vertsBot = [
(-self.ratio, ydiff - pitchdiff),
(-self.ratio, -1),
(self.ratio, -1),
(self.ratio, -ydiff - pitchdiff),
(-self.ratio, ydiff - pitchdiff),
]
self.botPolygon.set_xy(vertsBot)
def createPitchMarkers(self):
"""Creates the rectangle patches for the pitch indicators."""
self.pitchPatches = []
# Major Lines (multiple of 10 deg)
for i in [-9, -8, -7, -6, -5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9]:
width = self.calcPitchMarkerWidth(i)
currPatch = patches.Rectangle(
(-width / 2.0, self.dist10deg * i - (self.thick / 2.0)),
width,
self.thick,
facecolor="w",
edgecolor="none",
)
self.axes.add_patch(currPatch)
self.pitchPatches.append(currPatch)
# Add Label for +-30 deg
self.vertSize = 0.09
self.pitchLabelsLeft = []
self.pitchLabelsRight = []
i = 0
for j in [-90, -60, -30, 30, 60, 90]:
self.pitchLabelsLeft.append(
self.axes.text(
-0.55,
(j / 10.0) * self.dist10deg,
str(j),
color="w",
size=self.fontSize,
horizontalalignment="center",
verticalalignment="center",
)
)
self.pitchLabelsLeft[i].set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.pitchLabelsRight.append(
self.axes.text(
0.55,
(j / 10.0) * self.dist10deg,
str(j),
color="w",
size=self.fontSize,
horizontalalignment="center",
verticalalignment="center",
)
)
self.pitchLabelsRight[i].set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
i += 1
def calcPitchMarkerWidth(self, i):
"""Calculates the width of a pitch marker."""
if (i % 3) == 0:
if i == 0:
width = 1.5
else:
width = 0.9
else:
width = 0.6
return width
def adjustPitchmarkers(self):
"""Adjusts the location and orientation of pitch markers."""
pitchdiff = self.dist10deg * (self.pitch / 10.0)
rollRotate = mpl.transforms.Affine2D().rotate_deg_around(0.0, -pitchdiff, self.roll) + self.axes.transData
j = 0
for i in [-9, -8, -7, -6, -5, -4, -3, -2, -1, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9]:
width = self.calcPitchMarkerWidth(i)
self.pitchPatches[j].set_xy((-width / 2.0, self.dist10deg * i - (self.thick / 2.0) - pitchdiff))
self.pitchPatches[j].set_transform(rollRotate)
j += 1
# Adjust Text Size and rotation
i = 0
for j in [-9, -6, -3, 3, 6, 9]:
self.pitchLabelsLeft[i].set_y(j * self.dist10deg - pitchdiff)
self.pitchLabelsRight[i].set_y(j * self.dist10deg - pitchdiff)
self.pitchLabelsLeft[i].set_size(self.fontSize)
self.pitchLabelsRight[i].set_size(self.fontSize)
self.pitchLabelsLeft[i].set_rotation(self.roll)
self.pitchLabelsRight[i].set_rotation(self.roll)
self.pitchLabelsLeft[i].set_transform(rollRotate)
self.pitchLabelsRight[i].set_transform(rollRotate)
i += 1
def createAARText(self):
"""Creates the text for airspeed, altitude and climb rate."""
self.airspeedText = self.axes.text(
self.rightPos - (self.vertSize / 10.0),
-0.97 + (2 * self.vertSize) - (self.vertSize / 10.0),
"AS: %.1f m/s" % self.airspeed,
color="w",
size=self.fontSize,
ha="right",
)
self.altitudeText = self.axes.text(
self.rightPos - (self.vertSize / 10.0),
-0.97 + self.vertSize - (0.5 * self.vertSize / 10.0),
"ALT: %.1f m " % self.relAlt,
color="w",
size=self.fontSize,
ha="right",
)
self.climbRateText = self.axes.text(
self.rightPos - (self.vertSize / 10.0),
-0.97,
"CR: %.1f m/s" % self.climbRate,
color="w",
size=self.fontSize,
ha="right",
)
self.airspeedText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.altitudeText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.climbRateText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
def updateAARLocations(self):
"""Update the locations of airspeed, altitude and Climb rate."""
# Locations
self.airspeedText.set_position(
(self.rightPos - (self.vertSize / 10.0), -0.97 + (2 * self.vertSize) - (self.vertSize / 10.0))
)
self.altitudeText.set_position(
(self.rightPos - (self.vertSize / 10.0), -0.97 + self.vertSize - (0.5 * self.vertSize / 10.0))
)
self.climbRateText.set_position((self.rightPos - (self.vertSize / 10.0), -0.97))
# Font Size
self.airspeedText.set_size(self.fontSize)
self.altitudeText.set_size(self.fontSize)
self.climbRateText.set_size(self.fontSize)
def updateAARText(self):
"Updates the displayed airspeed, altitude, climb rate Text"
self.airspeedText.set_text("AR: %.1f m/s" % self.airspeed)
self.altitudeText.set_text("ALT: %.1f m " % self.relAlt)
self.climbRateText.set_text("CR: %.1f m/s" % self.climbRate)
def createBatteryBar(self):
"""Creates the bar to display current battery percentage."""
self.batOutRec = patches.Rectangle(
(self.rightPos - (1.3 + self.rOffset) * self.batWidth, 1.0 - (0.1 + 1.0 + (2 * 0.075)) * self.batHeight),
self.batWidth * 1.3,
self.batHeight * 1.15,
facecolor="darkgrey",
edgecolor="none",
)
self.batInRec = patches.Rectangle(
(self.rightPos - (self.rOffset + 1 + 0.15) * self.batWidth, 1.0 - (0.1 + 1 + 0.075) * self.batHeight),
self.batWidth,
self.batHeight,
facecolor="lawngreen",
edgecolor="none",
)
self.batPerText = self.axes.text(
self.rightPos - (self.rOffset + 0.65) * self.batWidth,
1 - (0.1 + 1 + (0.075 + 0.15)) * self.batHeight,
"%.f" % self.batRemain,
color="w",
size=self.fontSize,
ha="center",
va="top",
)
self.batPerText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.voltsText = self.axes.text(
self.rightPos - (self.rOffset + 1.3 + 0.2) * self.batWidth,
1 - (0.1 + 0.05 + 0.075) * self.batHeight,
"%.1f V" % self.voltage,
color="w",
size=self.fontSize,
ha="right",
va="top",
)
self.ampsText = self.axes.text(
self.rightPos - (self.rOffset + 1.3 + 0.2) * self.batWidth,
1 - self.vertSize - (0.1 + 0.05 + 0.1 + 0.075) * self.batHeight,
"%.1f A" % self.current,
color="w",
size=self.fontSize,
ha="right",
va="top",
)
self.voltsText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.ampsText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
self.axes.add_patch(self.batOutRec)
self.axes.add_patch(self.batInRec)
def updateBatteryBar(self):
"""Updates the position and values of the battery bar."""
# Bar
self.batOutRec.set_xy(
(self.rightPos - (1.3 + self.rOffset) * self.batWidth, 1.0 - (0.1 + 1.0 + (2 * 0.075)) * self.batHeight)
)
self.batInRec.set_xy(
(self.rightPos - (self.rOffset + 1 + 0.15) * self.batWidth, 1.0 - (0.1 + 1 + 0.075) * self.batHeight)
)
self.batPerText.set_position(
(self.rightPos - (self.rOffset + 0.65) * self.batWidth, 1 - (0.1 + 1 + (0.075 + 0.15)) * self.batHeight)
)
self.batPerText.set_fontsize(self.fontSize)
self.voltsText.set_text("%.1f V" % self.voltage)
self.ampsText.set_text("%.1f A" % self.current)
self.voltsText.set_position(
(self.rightPos - (self.rOffset + 1.3 + 0.2) * self.batWidth, 1 - (0.1 + 0.05) * self.batHeight)
)
self.ampsText.set_position(
(
self.rightPos - (self.rOffset + 1.3 + 0.2) * self.batWidth,
1 - self.vertSize - (0.1 + 0.05 + 0.1) * self.batHeight,
)
)
self.voltsText.set_fontsize(self.fontSize)
self.ampsText.set_fontsize(self.fontSize)
if self.batRemain >= 0:
self.batPerText.set_text(int(self.batRemain))
self.batInRec.set_height(self.batRemain * self.batHeight / 100.0)
if self.batRemain / 100.0 > 0.5:
self.batInRec.set_facecolor("lawngreen")
elif self.batRemain / 100.0 <= 0.5 and self.batRemain / 100.0 > 0.2:
self.batInRec.set_facecolor("yellow")
elif self.batRemain / 100.0 <= 0.2 and self.batRemain >= 0.0:
self.batInRec.set_facecolor("r")
elif self.batRemain == -1:
self.batInRec.set_height(self.batHeight)
self.batInRec.set_facecolor("k")
def createStateText(self):
"""Creates the mode and arm state text."""
self.modeText = self.axes.text(
self.leftPos + (self.vertSize / 10.0),
0.97,
"UNKNOWN",
color="grey",
size=1.5 * self.fontSize,
ha="left",
va="top",
)
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=self.fontSize / 10.0, foreground="black")])
def updateStateText(self):
"""Updates the mode and colours red or green depending on arm state."""
self.modeText.set_position((self.leftPos + (self.vertSize / 10.0), 0.97))
self.modeText.set_text(self.mode)
self.modeText.set_size(1.5 * self.fontSize)
if self.armed:
self.modeText.set_color("red")
self.modeText.set_path_effects(
[PathEffects.withStroke(linewidth=self.fontSize / 10.0, foreground="yellow")]
)
elif self.armed == False:
self.modeText.set_color("lightgreen")
self.modeText.set_bbox(None)
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="black")])
else:
# Fall back if unknown
self.modeText.set_color("grey")
self.modeText.set_bbox(None)
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=self.fontSize / 10.0, foreground="black")])
def createWPText(self):
"""Creates the text for the current and final waypoint,
and the distance to the new waypoint."""
self.wpText = self.axes.text(
self.leftPos + (1.5 * self.vertSize / 10.0),
0.97 - (1.5 * self.vertSize) + (0.5 * self.vertSize / 10.0),
"0/0\n(0 m, 0 s)",
color="w",
size=self.fontSize,
ha="left",
va="top",
)
self.wpText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="black")])
def updateWPText(self):
"""Updates the current waypoint and distance to it."""
self.wpText.set_position(
(self.leftPos + (1.5 * self.vertSize / 10.0), 0.97 - (1.5 * self.vertSize) + (0.5 * self.vertSize / 10.0))
)
self.wpText.set_size(self.fontSize)
if type(self.nextWPTime) is str:
self.wpText.set_text("%.f/%.f\n(%.f m, ~ s)" % (self.currentWP, self.finalWP, self.wpDist))
else:
self.wpText.set_text(
"%.f/%.f\n(%.f m, %.f s)" % (self.currentWP, self.finalWP, self.wpDist, self.nextWPTime)
)
def createWPPointer(self):
"""Creates the waypoint pointer relative to current heading."""
self.headingWPTri = patches.RegularPolygon((0.0, 0.55), 3, 0.05, facecolor="lime", zorder=4, ec="k")
self.axes.add_patch(self.headingWPTri)
self.headingWPText = self.axes.text(
0.0,
0.45,
"1",
color="lime",
size=self.fontSize,
horizontalalignment="center",
verticalalignment="center",
zorder=4,
)
self.headingWPText.set_path_effects([PathEffects.withStroke(linewidth=1, foreground="k")])
def adjustWPPointer(self):
"""Adjust the position and orientation of
the waypoint pointer."""
self.headingWPText.set_size(self.fontSize)
headingRotate = (
mpl.transforms.Affine2D().rotate_deg_around(0.0, 0.0, -self.wpBearing + self.heading) + self.axes.transData
)
self.headingWPText.set_transform(headingRotate)
angle = self.wpBearing - self.heading
if angle < 0:
angle += 360
if (angle > 90) and (angle < 270):
headRot = angle - 180
else:
headRot = angle
self.headingWPText.set_rotation(-headRot)
self.headingWPTri.set_transform(headingRotate)
self.headingWPText.set_text("%.f" % (angle))
def createAltHistoryPlot(self):
"""Creates the altitude history plot."""
self.altHistRect = patches.Rectangle(
(self.leftPos + (self.vertSize / 10.0), -0.25),
0.5,
0.5,
facecolor="grey",
edgecolor="none",
alpha=0.4,
zorder=4,
)
self.axes.add_patch(self.altHistRect)
self.altPlot, = self.axes.plot(
[self.leftPos + (self.vertSize / 10.0), self.leftPos + (self.vertSize / 10.0) + 0.5],
[0.0, 0.0],
color="k",
marker=None,
zorder=4,
)
self.altMarker, = self.axes.plot(
self.leftPos + (self.vertSize / 10.0) + 0.5, 0.0, marker="o", color="k", zorder=4
)
self.altText2 = self.axes.text(
self.leftPos + (4 * self.vertSize / 10.0) + 0.5,
0.0,
"%.f m" % self.relAlt,
color="k",
size=self.fontSize,
ha="left",
va="center",
zorder=4,
)
def updateAltHistory(self):
"""Updates the altitude history plot."""
self.altHist.append(self.relAlt)
self.timeHist.append(time.time())
# Delete entries older than x seconds
histLim = 10
currentTime = time.time()
point = 0
for i in range(0, len(self.timeHist)):
if self.timeHist[i] > (currentTime - 10.0):
break
# Remove old entries
self.altHist = self.altHist[i:]
self.timeHist = self.timeHist[i:]
# Transform Data
x = []
y = []
tmin = min(self.timeHist)
tmax = max(self.timeHist)
x1 = self.leftPos + (self.vertSize / 10.0)
y1 = -0.25
altMin = 0
altMax = max(self.altHist)
# Keep alt max for whole mission
if altMax > self.altMax:
self.altMax = altMax
else:
altMax = self.altMax
if tmax != tmin:
mx = 0.5 / (tmax - tmin)
else:
mx = 0.0
if altMax != altMin:
my = 0.5 / (altMax - altMin)
else:
my = 0.0
for t in self.timeHist:
x.append(mx * (t - tmin) + x1)
for alt in self.altHist:
val = my * (alt - altMin) + y1
# Crop extreme noise
if val < -0.25:
val = -0.25
elif val > 0.25:
val = 0.25
y.append(val)
# Display Plot
self.altHistRect.set_x(self.leftPos + (self.vertSize / 10.0))
self.altPlot.set_data(x, y)
self.altMarker.set_data(self.leftPos + (self.vertSize / 10.0) + 0.5, val)
self.altText2.set_position((self.leftPos + (4 * self.vertSize / 10.0) + 0.5, val))
self.altText2.set_size(self.fontSize)
self.altText2.set_text("%.f m" % self.relAlt)
# =============== Event Bindings =============== #
def on_idle(self, event):
"""To adjust text and positions on rescaling the window when resized."""
# Check for resize
self.checkReszie()
if self.resized:
# Fix Window Scales
self.rescaleX()
self.calcFontScaling()
# Recalculate Horizon Polygons
self.calcHorizonPoints()
# Update Roll, Pitch, Yaw Text Locations
self.updateRPYLocations()
# Update Airpseed, Altitude, Climb Rate Locations
self.updateAARLocations()
# Update Pitch Markers
self.adjustPitchmarkers()
# Update Heading and North Pointer
self.adjustHeadingPointer()
self.adjustNorthPointer()
# Update Battery Bar
self.updateBatteryBar()
# Update Mode and State
self.updateStateText()
# Update Waypoint Text
self.updateWPText()
# Adjust Waypoint Pointer
self.adjustWPPointer()
# Update History Plot
self.updateAltHistory()
# Update Matplotlib Plot
self.canvas.draw()
self.canvas.Refresh()
self.resized = False
time.sleep(0.05)
def on_timer(self, event):
"""Main Loop."""
state = self.state
if state.close_event.wait(0.001):
self.timer.Stop()
self.Destroy()
return
# Check for resizing
self.checkReszie()
if self.resized:
self.on_idle(0)
# Get attitude information
while state.child_pipe_recv.poll():
obj = state.child_pipe_recv.recv()
self.calcFontScaling()
if isinstance(obj, Attitude):
self.oldRoll = self.roll
self.pitch = obj.pitch * 180 / math.pi
self.roll = obj.roll * 180 / math.pi
self.yaw = obj.yaw * 180 / math.pi
# Update Roll, Pitch, Yaw Text Text
self.updateRPYText()
# Recalculate Horizon Polygons
self.calcHorizonPoints()
# Update Pitch Markers
self.adjustPitchmarkers()
elif isinstance(obj, VFR_HUD):
self.heading = obj.heading
self.airspeed = obj.airspeed
self.climbRate = obj.climbRate
# Update Airpseed, Altitude, Climb Rate Locations
self.updateAARText()
# Update Heading North Pointer
self.adjustHeadingPointer()
self.adjustNorthPointer()
elif isinstance(obj, Global_Position_INT):
self.relAlt = obj.relAlt
# Update Airpseed, Altitude, Climb Rate Locations
self.updateAARText()
# Update Altitude History
self.updateAltHistory()
elif isinstance(obj, BatteryInfo):
self.voltage = obj.voltage
self.current = obj.current
self.batRemain = obj.batRemain
# Update Battery Bar
self.updateBatteryBar()
elif isinstance(obj, FlightState):
self.mode = obj.mode
self.armed = obj.armState
# Update Mode and Arm State Text
self.updateStateText()
elif isinstance(obj, WaypointInfo):
self.currentWP = obj.current
self.finalWP = obj.final
self.wpDist = obj.currentDist
self.nextWPTime = obj.nextWPTime
if obj.wpBearing < 0.0:
self.wpBearing = obj.wpBearing + 360
else:
self.wpBearing = obj.wpBearing
# Update waypoint text
self.updateWPText()
# Adjust Waypoint Pointer
self.adjustWPPointer()
# Update Matplotlib Plot
self.canvas.draw()
self.canvas.Refresh()
self.Refresh()
self.Update()
def on_KeyPress(self, event):
"""To adjust the distance between pitch markers."""
if event.GetKeyCode() == wx.WXK_UP:
self.dist10deg += 0.1
print "Dist per 10 deg: %.1f" % self.dist10deg
elif event.GetKeyCode() == wx.WXK_DOWN:
self.dist10deg -= 0.1
if self.dist10deg <= 0:
self.dist10deg = 0.1
print "Dist per 10 deg: %.1f" % self.dist10deg
# Toggle Widgets
elif event.GetKeyCode() == 49: # 1
widgets = [self.modeText, self.wpText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 50: # 2
widgets = [self.batOutRec, self.batInRec, self.voltsText, self.ampsText, self.batPerText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 51: # 3
widgets = [self.rollText, self.pitchText, self.yawText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 52: # 4
widgets = [self.airspeedText, self.altitudeText, self.climbRateText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 53: # 5
widgets = [self.altHistRect, self.altPlot, self.altMarker, self.altText2]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 54: # 6
widgets = [
self.headingTri,
self.headingText,
self.headingNorthTri,
self.headingNorthText,
self.headingWPTri,
self.headingWPText,
]
self.toggleWidgets(widgets)
# Update Matplotlib Plot
self.canvas.draw()
self.canvas.Refresh()
self.Refresh()
self.Update()
def __init__(self, **kwargs):
Shape.all_shapes.append(self)
self.exc = False
dummy_xy = np.array([[0, 0], [0, 1], [1, 1]])
# clone constructor
if len(kwargs) == 1:
init_shape = kwargs['init_shape']
assert isinstance(init_shape, Shape)
def clone_patch(init_patch):
if init_patch is None:
return None
else:
result = Polygon(xy=dummy_xy,
closed=init_patch.get_closed())
result.update_from(other=init_patch)
result.set_xy(np.copy(init_patch.get_xy()))
init_patch.axes.add_patch(result)
assert is_the_same_contour(p1=result.get_xy(),
p2=init_patch.get_xy())
return result
self.patch = clone_patch(init_patch=init_shape.patch)
self.outline = clone_patch(init_patch=init_shape.outline)
self.line = clone_patch(init_patch=init_shape.line)
self.diamond = clone_patch(init_patch=init_shape.diamond)
self.shapename = init_shape.shapename
self.clip_patches = copy.deepcopy(init_shape.clip_patches)
self.move_matrix = init_shape.move_matrix.copy()
self.shape_kwargs = copy.deepcopy(init_shape.shape_kwargs)
self.diamond_coords = np.copy(init_shape.diamond_coords)
# from-scratch constructor
elif len(kwargs) == 2:
ax = kwargs['ax']
shapetype = kwargs['shapetype']
assert shapetype in ["patch", "line"]
self.diamond_coords = np.array([0, 0])
diamond_size = get_diamond_size(ax)
diamond_contour = np.array([[1, 0], [0, -1], [-1, 0], [
0, 1
]]) * diamond_size if diamond_size is not None else None
self.patch = Polygon(xy=dummy_xy, fill=True,
closed=True) if shapetype == "patch" else None
self.outline = Polygon(
xy=dummy_xy, fill=False,
closed=True) if shapetype == "patch" else None
self.line = Polygon(xy=dummy_xy, fill=False,
closed=False) if shapetype == "line" else None
self.diamond = Polygon(
xy=diamond_contour, fill=True,
closed=True) if diamond_size is not None else None
for what in [self.patch, self.outline, self.line, self.diamond]:
if what is not None:
gca().add_patch(what)
for attr_name in format_arg_dict.keys():
_attr = getattr(self, attr_name)
if _attr is not None:
set_style(_attr, attr_name)
self.clip_patches = []
self.move_matrix = np.array([[1, 0], [0, 1]])
self.shape_kwargs = {}
self.shapename = None
else:
raise Exception("Constructor arguments are invalid")
def plot_ffield(mc_obj):
"""
Plots 2D geometry of lidar scan and flow field box
Parameters
----------
mc_obj : mocalum
Instance of mocalum class
"""
flow_id = mc_obj.data.ffield.generator
lidar_ids = mc_obj.data.bbox_ffield[flow_id]['linked_lidars']
no_los = mc_obj.data.meas_cfg[lidar_ids[0]]['config']['no_los']
meas_pts = mc_obj._get_prob_cords(lidar_ids[0])[:no_los]
bbox_pts = bbox_pts_from_cfg(mc_obj.data.bbox_ffield[flow_id])
diag = np.abs(bbox_pts[0] - bbox_pts[2]).max()
R_tb = mc_obj.data.bbox_ffield[flow_id]['CRS']['rot_matrix']
min_bbox_pts = bbox_pts.dot(inv(R_tb))
bbox_c = min_bbox_pts.mean(axis=0)
wind_dir = mc_obj.data.fmodel_cfg['wind_from_direction']
flowbox = Polygon(min_bbox_pts,
alpha=0.4,
color='grey',
label="flow field bbox")
wind_dir_pt = spher2cart(wind_dir, 0, diag / 2)[:2]
lidar_pos = []
for id in lidar_ids:
lidar_pos += [mc_obj.data.meas_cfg[id]['position']]
fig, ax = plt.subplots(figsize=(7.5, 7.5))
plt.grid()
mc_obj.data.ffield.u.isel(z=0, time=0).plot.pcolormesh('Easting',
'Northing',
ax=ax,
cmap='Greys')
plt.arrow(bbox_c[0],
bbox_c[1],
-wind_dir_pt[0],
-wind_dir_pt[1],
width=8,
color="red",
label='wind',
zorder=50)
plt.scatter(meas_pts[:, 0],
meas_pts[:, 1],
c="blue",
label='measurements',
zorder=10)
colors = ["green", "orange", "purple"]
for j, id in enumerate(lidar_ids):
plt.scatter(lidar_pos[j][0],
lidar_pos[j][1],
c=colors[j],
label=id,
zorder=150)
for i, pt in enumerate(meas_pts):
if i == 0 and j == 0:
plt.plot([lidar_pos[j][0], pt[0]], [lidar_pos[j][1], pt[1]],
c='black',
alpha=0.4,
label='beam')
else:
plt.plot([lidar_pos[j][0], pt[0]], [lidar_pos[j][1], pt[1]],
c='black',
alpha=0.4)
ax.add_patch(flowbox)
ax.set_aspect('equal')
plt.legend(loc="lower right")
plt.xlabel('Easting [m]')
plt.ylabel('Northing [m]')
plt.show()
class HorizonFrame(wx.Frame):
""" The main frame of the horizon indicator."""
def __init__(self, state, title):
self.state = state
# Create Frame and Panel(s)
wx.Frame.__init__(self, None, title=title)
state.frame = self
# Initialisation
self.initData()
self.initUI()
self.startTime = time.time()
self.nextTime = 0.0
self.fps = 10.0
def initData(self):
# Initialise Attitude
self.pitch = 0.0 # Degrees
self.roll = 0.0 # Degrees
self.yaw = 0.0 # Degrees
# History Values
self.oldRoll = 0.0 # Degrees
# Initialise Rate Information
self.airspeed = 0.0 # m/s
self.relAlt = 0.0 # m relative to home position
self.relAltTime = 0.0 # s The time that the relative altitude was recorded
self.climbRate = 0.0 # m/s
self.altHist = [] # Altitude History
self.timeHist = [] # Time History
self.altMax = 0.0 # Maximum altitude since startup
# Initialise HUD Info
self.heading = 0.0 # 0-360
# Initialise Battery Info
self.voltage = 0.0
self.current = 0.0
self.batRemain = 0.0
# Initialise Mode and State
self.mode = 'UNKNOWN'
self.armed = ''
self.safetySwitch = ''
# Intialise Waypoint Information
self.currentWP = 0
self.finalWP = 0
self.wpDist = 0
self.nextWPTime = 0
self.wpBearing = 0
def initUI(self):
# Create Event Timer and Bindings
self.timer = wx.Timer(self)
self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
self.timer.Start(100)
self.Bind(wx.EVT_IDLE, self.on_idle)
self.Bind(wx.EVT_CHAR_HOOK,self.on_KeyPress)
# Create Panel
self.panel = wx.Panel(self)
self.vertSize = 0.09
self.resized = False
# Create Matplotlib Panel
self.createPlotPanel()
# Fix Axes - vertical is of length 2, horizontal keeps the same lengthscale
self.rescaleX()
self.calcFontScaling()
# Create Horizon Polygons
self.createHorizonPolygons()
# Center Pointer Marker
self.thick = 0.015
self.createCenterPointMarker()
# Pitch Markers
self.dist10deg = 0.2 # Graph distance per 10 deg
self.createPitchMarkers()
# Add Roll, Pitch, Yaw Text
self.createRPYText()
# Add Airspeed, Altitude, Climb Rate Text
self.createAARText()
# Create Heading Pointer
self.createHeadingPointer()
# Create North Pointer
self.createNorthPointer()
# Create Battery Bar
self.batWidth = 0.1
self.batHeight = 0.2
self.rOffset = 0.35
self.createBatteryBar()
# Create Mode & State Text
self.createStateText()
# Create Waypoint Text
self.createWPText()
# Create Waypoint Pointer
self.createWPPointer()
# Create Altitude History Plot
self.createAltHistoryPlot()
# Show Frame
self.Show(True)
self.pending = []
def createPlotPanel(self):
'''Creates the figure and axes for the plotting panel.'''
self.figure = Figure()
self.axes = self.figure.add_subplot(111)
self.canvas = FigureCanvas(self,-1,self.figure)
self.canvas.SetSize(wx.Size(300,300))
self.axes.axis('off')
self.figure.subplots_adjust(left=0,right=1,top=1,bottom=0)
self.sizer = wx.BoxSizer(wx.VERTICAL)
self.sizer.Add(self.canvas,1,wx.EXPAND,wx.ALL)
self.SetSizerAndFit(self.sizer)
self.Fit()
def rescaleX(self):
'''Rescales the horizontal axes to make the lengthscales equal.'''
self.ratio = self.figure.get_size_inches()[0]/float(self.figure.get_size_inches()[1])
self.axes.set_xlim(-self.ratio,self.ratio)
self.axes.set_ylim(-1,1)
def calcFontScaling(self):
'''Calculates the current font size and left position for the current window.'''
self.ypx = self.figure.get_size_inches()[1]*self.figure.dpi
self.xpx = self.figure.get_size_inches()[0]*self.figure.dpi
self.fontSize = self.vertSize*(self.ypx/2.0)
self.leftPos = self.axes.get_xlim()[0]
self.rightPos = self.axes.get_xlim()[1]
def checkReszie(self):
'''Checks if the window was resized.'''
if not self.resized:
oldypx = self.ypx
oldxpx = self.xpx
self.ypx = self.figure.get_size_inches()[1]*self.figure.dpi
self.xpx = self.figure.get_size_inches()[0]*self.figure.dpi
if (oldypx != self.ypx) or (oldxpx != self.xpx):
self.resized = True
else:
self.resized = False
def createHeadingPointer(self):
'''Creates the pointer for the current heading.'''
self.headingTri = patches.RegularPolygon((0.0,0.80),3,0.05,color='k',zorder=4)
self.axes.add_patch(self.headingTri)
self.headingText = self.axes.text(0.0,0.675,'0',color='k',size=self.fontSize,horizontalalignment='center',verticalalignment='center',zorder=4)
def adjustHeadingPointer(self):
'''Adjust the value of the heading pointer.'''
self.headingText.set_text(str(self.heading))
self.headingText.set_size(self.fontSize)
def createNorthPointer(self):
'''Creates the north pointer relative to current heading.'''
self.headingNorthTri = patches.RegularPolygon((0.0,0.80),3,0.05,color='k',zorder=4)
self.axes.add_patch(self.headingNorthTri)
self.headingNorthText = self.axes.text(0.0,0.675,'N',color='k',size=self.fontSize,horizontalalignment='center',verticalalignment='center',zorder=4)
def adjustNorthPointer(self):
'''Adjust the position and orientation of
the north pointer.'''
self.headingNorthText.set_size(self.fontSize)
headingRotate = mpl.transforms.Affine2D().rotate_deg_around(0.0,0.0,self.heading)+self.axes.transData
self.headingNorthText.set_transform(headingRotate)
if (self.heading > 90) and (self.heading < 270):
headRot = self.heading-180
else:
headRot = self.heading
self.headingNorthText.set_rotation(headRot)
self.headingNorthTri.set_transform(headingRotate)
# Adjust if overlapping with heading pointer
if (self.heading <= 10.0) or (self.heading >= 350.0):
self.headingNorthText.set_text('')
else:
self.headingNorthText.set_text('N')
def toggleWidgets(self,widgets):
'''Hides/shows the given widgets.'''
for wig in widgets:
if wig.get_visible():
wig.set_visible(False)
else:
wig.set_visible(True)
def createRPYText(self):
'''Creates the text for roll, pitch and yaw.'''
self.rollText = self.axes.text(self.leftPos+(self.vertSize/10.0),-0.97+(2*self.vertSize)-(self.vertSize/10.0),'Roll: %.2f' % self.roll,color='w',size=self.fontSize)
self.pitchText = self.axes.text(self.leftPos+(self.vertSize/10.0),-0.97+self.vertSize-(0.5*self.vertSize/10.0),'Pitch: %.2f' % self.pitch,color='w',size=self.fontSize)
self.yawText = self.axes.text(self.leftPos+(self.vertSize/10.0),-0.97,'Yaw: %.2f' % self.yaw,color='w',size=self.fontSize)
self.rollText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.pitchText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.yawText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
def updateRPYLocations(self):
'''Update the locations of roll, pitch, yaw text.'''
# Locations
self.rollText.set_position((self.leftPos+(self.vertSize/10.0),-0.97+(2*self.vertSize)-(self.vertSize/10.0)))
self.pitchText.set_position((self.leftPos+(self.vertSize/10.0),-0.97+self.vertSize-(0.5*self.vertSize/10.0)))
self.yawText.set_position((self.leftPos+(self.vertSize/10.0),-0.97))
# Font Size
self.rollText.set_size(self.fontSize)
self.pitchText.set_size(self.fontSize)
self.yawText.set_size(self.fontSize)
def updateRPYText(self):
'Updates the displayed Roll, Pitch, Yaw Text'
self.rollText.set_text('Roll: %.2f' % self.roll)
self.pitchText.set_text('Pitch: %.2f' % self.pitch)
self.yawText.set_text('Yaw: %.2f' % self.yaw)
def createCenterPointMarker(self):
'''Creates the center pointer in the middle of the screen.'''
self.axes.add_patch(patches.Rectangle((-0.75,-self.thick),0.5,2.0*self.thick,facecolor='orange',zorder=3))
self.axes.add_patch(patches.Rectangle((0.25,-self.thick),0.5,2.0*self.thick,facecolor='orange',zorder=3))
self.axes.add_patch(patches.Circle((0,0),radius=self.thick,facecolor='orange',edgecolor='none',zorder=3))
def createHorizonPolygons(self):
'''Creates the two polygons to show the sky and ground.'''
# Sky Polygon
vertsTop = [[-1,0],[-1,1],[1,1],[1,0],[-1,0]]
self.topPolygon = Polygon(vertsTop,facecolor='dodgerblue',edgecolor='none')
self.axes.add_patch(self.topPolygon)
# Ground Polygon
vertsBot = [[-1,0],[-1,-1],[1,-1],[1,0],[-1,0]]
self.botPolygon = Polygon(vertsBot,facecolor='brown',edgecolor='none')
self.axes.add_patch(self.botPolygon)
def calcHorizonPoints(self):
'''Updates the verticies of the patches for the ground and sky.'''
ydiff = math.tan(math.radians(-self.roll))*float(self.ratio)
pitchdiff = self.dist10deg*(self.pitch/10.0)
# Sky Polygon
vertsTop = [(-self.ratio,ydiff-pitchdiff),(-self.ratio,1),(self.ratio,1),(self.ratio,-ydiff-pitchdiff),(-self.ratio,ydiff-pitchdiff)]
self.topPolygon.set_xy(vertsTop)
# Ground Polygon
vertsBot = [(-self.ratio,ydiff-pitchdiff),(-self.ratio,-1),(self.ratio,-1),(self.ratio,-ydiff-pitchdiff),(-self.ratio,ydiff-pitchdiff)]
self.botPolygon.set_xy(vertsBot)
def createPitchMarkers(self):
'''Creates the rectangle patches for the pitch indicators.'''
self.pitchPatches = []
# Major Lines (multiple of 10 deg)
for i in [-9,-8,-7,-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6,7,8,9]:
width = self.calcPitchMarkerWidth(i)
currPatch = patches.Rectangle((-width/2.0,self.dist10deg*i-(self.thick/2.0)),width,self.thick,facecolor='w',edgecolor='none')
self.axes.add_patch(currPatch)
self.pitchPatches.append(currPatch)
# Add Label for +-30 deg
self.vertSize = 0.09
self.pitchLabelsLeft = []
self.pitchLabelsRight = []
i=0
for j in [-90,-60,-30,30,60,90]:
self.pitchLabelsLeft.append(self.axes.text(-0.55,(j/10.0)*self.dist10deg,str(j),color='w',size=self.fontSize,horizontalalignment='center',verticalalignment='center'))
self.pitchLabelsLeft[i].set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.pitchLabelsRight.append(self.axes.text(0.55,(j/10.0)*self.dist10deg,str(j),color='w',size=self.fontSize,horizontalalignment='center',verticalalignment='center'))
self.pitchLabelsRight[i].set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
i += 1
def calcPitchMarkerWidth(self,i):
'''Calculates the width of a pitch marker.'''
if (i % 3) == 0:
if i == 0:
width = 1.5
else:
width = 0.9
else:
width = 0.6
return width
def adjustPitchmarkers(self):
'''Adjusts the location and orientation of pitch markers.'''
pitchdiff = self.dist10deg*(self.pitch/10.0)
rollRotate = mpl.transforms.Affine2D().rotate_deg_around(0.0,-pitchdiff,self.roll)+self.axes.transData
j=0
for i in [-9,-8,-7,-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6,7,8,9]:
width = self.calcPitchMarkerWidth(i)
self.pitchPatches[j].set_xy((-width/2.0,self.dist10deg*i-(self.thick/2.0)-pitchdiff))
self.pitchPatches[j].set_transform(rollRotate)
j+=1
# Adjust Text Size and rotation
i=0
for j in [-9,-6,-3,3,6,9]:
self.pitchLabelsLeft[i].set_y(j*self.dist10deg-pitchdiff)
self.pitchLabelsRight[i].set_y(j*self.dist10deg-pitchdiff)
self.pitchLabelsLeft[i].set_size(self.fontSize)
self.pitchLabelsRight[i].set_size(self.fontSize)
self.pitchLabelsLeft[i].set_rotation(self.roll)
self.pitchLabelsRight[i].set_rotation(self.roll)
self.pitchLabelsLeft[i].set_transform(rollRotate)
self.pitchLabelsRight[i].set_transform(rollRotate)
i += 1
def createAARText(self):
'''Creates the text for airspeed, altitude and climb rate.'''
self.airspeedText = self.axes.text(self.rightPos-(self.vertSize/10.0),-0.97+(2*self.vertSize)-(self.vertSize/10.0),'AS: %.1f m/s' % self.airspeed,color='w',size=self.fontSize,ha='right')
self.altitudeText = self.axes.text(self.rightPos-(self.vertSize/10.0),-0.97+self.vertSize-(0.5*self.vertSize/10.0),'ALT: %.1f m ' % self.relAlt,color='w',size=self.fontSize,ha='right')
self.climbRateText = self.axes.text(self.rightPos-(self.vertSize/10.0),-0.97,'CR: %.1f m/s' % self.climbRate,color='w',size=self.fontSize,ha='right')
self.airspeedText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.altitudeText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.climbRateText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
def updateAARLocations(self):
'''Update the locations of airspeed, altitude and Climb rate.'''
# Locations
self.airspeedText.set_position((self.rightPos-(self.vertSize/10.0),-0.97+(2*self.vertSize)-(self.vertSize/10.0)))
self.altitudeText.set_position((self.rightPos-(self.vertSize/10.0),-0.97+self.vertSize-(0.5*self.vertSize/10.0)))
self.climbRateText.set_position((self.rightPos-(self.vertSize/10.0),-0.97))
# Font Size
self.airspeedText.set_size(self.fontSize)
self.altitudeText.set_size(self.fontSize)
self.climbRateText.set_size(self.fontSize)
def updateAARText(self):
'Updates the displayed airspeed, altitude, climb rate Text'
self.airspeedText.set_text('AR: %.1f m/s' % self.airspeed)
self.altitudeText.set_text('ALT: %.1f m ' % self.relAlt)
self.climbRateText.set_text('CR: %.1f m/s' % self.climbRate)
def createBatteryBar(self):
'''Creates the bar to display current battery percentage.'''
self.batOutRec = patches.Rectangle((self.rightPos-(1.3+self.rOffset)*self.batWidth,1.0-(0.1+1.0+(2*0.075))*self.batHeight),self.batWidth*1.3,self.batHeight*1.15,facecolor='darkgrey',edgecolor='none')
self.batInRec = patches.Rectangle((self.rightPos-(self.rOffset+1+0.15)*self.batWidth,1.0-(0.1+1+0.075)*self.batHeight),self.batWidth,self.batHeight,facecolor='lawngreen',edgecolor='none')
self.batPerText = self.axes.text(self.rightPos - (self.rOffset+0.65)*self.batWidth,1-(0.1+1+(0.075+0.15))*self.batHeight,'%.f' % self.batRemain,color='w',size=self.fontSize,ha='center',va='top')
self.batPerText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.voltsText = self.axes.text(self.rightPos-(self.rOffset+1.3+0.2)*self.batWidth,1-(0.1+0.05+0.075)*self.batHeight,'%.1f V' % self.voltage,color='w',size=self.fontSize,ha='right',va='top')
self.ampsText = self.axes.text(self.rightPos-(self.rOffset+1.3+0.2)*self.batWidth,1-self.vertSize-(0.1+0.05+0.1+0.075)*self.batHeight,'%.1f A' % self.current,color='w',size=self.fontSize,ha='right',va='top')
self.voltsText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.ampsText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
self.axes.add_patch(self.batOutRec)
self.axes.add_patch(self.batInRec)
def updateBatteryBar(self):
'''Updates the position and values of the battery bar.'''
# Bar
self.batOutRec.set_xy((self.rightPos-(1.3+self.rOffset)*self.batWidth,1.0-(0.1+1.0+(2*0.075))*self.batHeight))
self.batInRec.set_xy((self.rightPos-(self.rOffset+1+0.15)*self.batWidth,1.0-(0.1+1+0.075)*self.batHeight))
self.batPerText.set_position((self.rightPos - (self.rOffset+0.65)*self.batWidth,1-(0.1+1+(0.075+0.15))*self.batHeight))
self.batPerText.set_fontsize(self.fontSize)
self.voltsText.set_text('%.1f V' % self.voltage)
self.ampsText.set_text('%.1f A' % self.current)
self.voltsText.set_position((self.rightPos-(self.rOffset+1.3+0.2)*self.batWidth,1-(0.1+0.05)*self.batHeight))
self.ampsText.set_position((self.rightPos-(self.rOffset+1.3+0.2)*self.batWidth,1-self.vertSize-(0.1+0.05+0.1)*self.batHeight))
self.voltsText.set_fontsize(self.fontSize)
self.ampsText.set_fontsize(self.fontSize)
if self.batRemain >= 0:
self.batPerText.set_text(int(self.batRemain))
self.batInRec.set_height(self.batRemain*self.batHeight/100.0)
if self.batRemain/100.0 > 0.5:
self.batInRec.set_facecolor('lawngreen')
elif self.batRemain/100.0 <= 0.5 and self.batRemain/100.0 > 0.2:
self.batInRec.set_facecolor('yellow')
elif self.batRemain/100.0 <= 0.2 and self.batRemain >= 0.0:
self.batInRec.set_facecolor('r')
elif self.batRemain == -1:
self.batInRec.set_height(self.batHeight)
self.batInRec.set_facecolor('k')
def createStateText(self):
'''Creates the mode and arm state text.'''
self.modeText = self.axes.text(self.leftPos+(self.vertSize/10.0),0.97,'UNKNOWN',color='grey',size=1.5*self.fontSize,ha='left',va='top')
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=self.fontSize/10.0,foreground='black')])
def updateStateText(self):
'''Updates the mode and colours red or green depending on arm state.'''
self.modeText.set_position((self.leftPos+(self.vertSize/10.0),0.97))
self.modeText.set_text(self.mode)
self.modeText.set_size(1.5*self.fontSize)
if self.armed:
self.modeText.set_color('red')
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=self.fontSize/10.0,foreground='yellow')])
elif (self.armed == False):
self.modeText.set_color('lightgreen')
self.modeText.set_bbox(None)
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='black')])
else:
# Fall back if unknown
self.modeText.set_color('grey')
self.modeText.set_bbox(None)
self.modeText.set_path_effects([PathEffects.withStroke(linewidth=self.fontSize/10.0,foreground='black')])
def createWPText(self):
'''Creates the text for the current and final waypoint,
and the distance to the new waypoint.'''
self.wpText = self.axes.text(self.leftPos+(1.5*self.vertSize/10.0),0.97-(1.5*self.vertSize)+(0.5*self.vertSize/10.0),'0/0\n(0 m, 0 s)',color='w',size=self.fontSize,ha='left',va='top')
self.wpText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='black')])
def updateWPText(self):
'''Updates the current waypoint and distance to it.'''
self.wpText.set_position((self.leftPos+(1.5*self.vertSize/10.0),0.97-(1.5*self.vertSize)+(0.5*self.vertSize/10.0)))
self.wpText.set_size(self.fontSize)
if type(self.nextWPTime) is str:
self.wpText.set_text('%.f/%.f\n(%.f m, ~ s)' % (self.currentWP,self.finalWP,self.wpDist))
else:
self.wpText.set_text('%.f/%.f\n(%.f m, %.f s)' % (self.currentWP,self.finalWP,self.wpDist,self.nextWPTime))
def createWPPointer(self):
'''Creates the waypoint pointer relative to current heading.'''
self.headingWPTri = patches.RegularPolygon((0.0,0.55),3,0.05,facecolor='lime',zorder=4,ec='k')
self.axes.add_patch(self.headingWPTri)
self.headingWPText = self.axes.text(0.0,0.45,'1',color='lime',size=self.fontSize,horizontalalignment='center',verticalalignment='center',zorder=4)
self.headingWPText.set_path_effects([PathEffects.withStroke(linewidth=1,foreground='k')])
def adjustWPPointer(self):
'''Adjust the position and orientation of
the waypoint pointer.'''
self.headingWPText.set_size(self.fontSize)
headingRotate = mpl.transforms.Affine2D().rotate_deg_around(0.0,0.0,-self.wpBearing+self.heading)+self.axes.transData
self.headingWPText.set_transform(headingRotate)
angle = self.wpBearing - self.heading
if angle < 0:
angle += 360
if (angle > 90) and (angle < 270):
headRot = angle-180
else:
headRot = angle
self.headingWPText.set_rotation(-headRot)
self.headingWPTri.set_transform(headingRotate)
self.headingWPText.set_text('%.f' % (angle))
def createAltHistoryPlot(self):
'''Creates the altitude history plot.'''
self.altHistRect = patches.Rectangle((self.leftPos+(self.vertSize/10.0),-0.25),0.5,0.5,facecolor='grey',edgecolor='none',alpha=0.4,zorder=4)
self.axes.add_patch(self.altHistRect)
self.altPlot, = self.axes.plot([self.leftPos+(self.vertSize/10.0),self.leftPos+(self.vertSize/10.0)+0.5],[0.0,0.0],color='k',marker=None,zorder=4)
self.altMarker, = self.axes.plot(self.leftPos+(self.vertSize/10.0)+0.5,0.0,marker='o',color='k',zorder=4)
self.altText2 = self.axes.text(self.leftPos+(4*self.vertSize/10.0)+0.5,0.0,'%.f m' % self.relAlt,color='k',size=self.fontSize,ha='left',va='center',zorder=4)
def updateAltHistory(self):
'''Updates the altitude history plot.'''
self.altHist.append(self.relAlt)
self.timeHist.append(self.relAltTime)
# Delete entries older than x seconds
histLim = 10
currentTime = time.time()
point = 0
for i in range(0,len(self.timeHist)):
if (self.timeHist[i] > (currentTime - 10.0)):
break
# Remove old entries
self.altHist = self.altHist[i:]
self.timeHist = self.timeHist[i:]
# Transform Data
x = []
y = []
tmin = min(self.timeHist)
tmax = max(self.timeHist)
x1 = self.leftPos+(self.vertSize/10.0)
y1 = -0.25
altMin = 0
altMax = max(self.altHist)
# Keep alt max for whole mission
if altMax > self.altMax:
self.altMax = altMax
else:
altMax = self.altMax
if tmax != tmin:
mx = 0.5/(tmax-tmin)
else:
mx = 0.0
if altMax != altMin:
my = 0.5/(altMax-altMin)
else:
my = 0.0
for t in self.timeHist:
x.append(mx*(t-tmin)+x1)
for alt in self.altHist:
val = my*(alt-altMin)+y1
# Crop extreme noise
if val < -0.25:
val = -0.25
elif val > 0.25:
val = 0.25
y.append(val)
# Display Plot
self.altHistRect.set_x(self.leftPos+(self.vertSize/10.0))
self.altPlot.set_data(x,y)
self.altMarker.set_data(self.leftPos+(self.vertSize/10.0)+0.5,val)
self.altText2.set_position((self.leftPos+(4*self.vertSize/10.0)+0.5,val))
self.altText2.set_size(self.fontSize)
self.altText2.set_text('%.f m' % self.relAlt)
# =============== Event Bindings =============== #
def on_idle(self, event):
'''To adjust text and positions on rescaling the window when resized.'''
# Check for resize
self.checkReszie()
if self.resized:
# Fix Window Scales
self.rescaleX()
self.calcFontScaling()
# Recalculate Horizon Polygons
self.calcHorizonPoints()
# Update Roll, Pitch, Yaw Text Locations
self.updateRPYLocations()
# Update Airpseed, Altitude, Climb Rate Locations
self.updateAARLocations()
# Update Pitch Markers
self.adjustPitchmarkers()
# Update Heading and North Pointer
self.adjustHeadingPointer()
self.adjustNorthPointer()
# Update Battery Bar
self.updateBatteryBar()
# Update Mode and State
self.updateStateText()
# Update Waypoint Text
self.updateWPText()
# Adjust Waypoint Pointer
self.adjustWPPointer()
# Update History Plot
self.updateAltHistory()
# Update Matplotlib Plot
self.canvas.draw()
self.canvas.Refresh()
self.resized = False
time.sleep(0.05)
def on_timer(self, event):
'''Main Loop.'''
state = self.state
self.loopStartTime = time.time()
if state.close_event.wait(0.001):
self.timer.Stop()
self.Destroy()
return
# Check for resizing
self.checkReszie()
if self.resized:
self.on_idle(0)
# Get attitude information
while state.child_pipe_recv.poll():
objList = state.child_pipe_recv.recv()
for obj in objList:
self.calcFontScaling()
if isinstance(obj,Attitude):
self.oldRoll = self.roll
self.pitch = obj.pitch*180/math.pi
self.roll = obj.roll*180/math.pi
self.yaw = obj.yaw*180/math.pi
# Update Roll, Pitch, Yaw Text Text
self.updateRPYText()
# Recalculate Horizon Polygons
self.calcHorizonPoints()
# Update Pitch Markers
self.adjustPitchmarkers()
elif isinstance(obj,VFR_HUD):
self.heading = obj.heading
self.airspeed = obj.airspeed
self.climbRate = obj.climbRate
# Update Airpseed, Altitude, Climb Rate Locations
self.updateAARText()
# Update Heading North Pointer
self.adjustHeadingPointer()
self.adjustNorthPointer()
elif isinstance(obj,Global_Position_INT):
self.relAlt = obj.relAlt
self.relAltTime = obj.curTime
# Update Airpseed, Altitude, Climb Rate Locations
self.updateAARText()
# Update Altitude History
self.updateAltHistory()
elif isinstance(obj,BatteryInfo):
self.voltage = obj.voltage
self.current = obj.current
self.batRemain = obj.batRemain
# Update Battery Bar
self.updateBatteryBar()
elif isinstance(obj,FlightState):
self.mode = obj.mode
self.armed = obj.armState
# Update Mode and Arm State Text
self.updateStateText()
elif isinstance(obj,WaypointInfo):
self.currentWP = obj.current
self.finalWP = obj.final
self.wpDist = obj.currentDist
self.nextWPTime = obj.nextWPTime
if obj.wpBearing < 0.0:
self.wpBearing = obj.wpBearing + 360
else:
self.wpBearing = obj.wpBearing
# Update waypoint text
self.updateWPText()
# Adjust Waypoint Pointer
self.adjustWPPointer()
elif isinstance(obj, FPS):
# Update fps target
self.fps = obj.fps
# Quit Drawing if too early
if (time.time() > self.nextTime):
# Update Matplotlib Plot
self.canvas.draw()
self.canvas.Refresh()
self.Refresh()
self.Update()
# Calculate next frame time
if (self.fps > 0):
fpsTime = 1/self.fps
self.nextTime = fpsTime + self.loopStartTime
else:
self.nextTime = time.time()
def on_KeyPress(self,event):
'''To adjust the distance between pitch markers.'''
if event.GetKeyCode() == wx.WXK_UP:
self.dist10deg += 0.1
print 'Dist per 10 deg: %.1f' % self.dist10deg
elif event.GetKeyCode() == wx.WXK_DOWN:
self.dist10deg -= 0.1
if self.dist10deg <= 0:
self.dist10deg = 0.1
print 'Dist per 10 deg: %.1f' % self.dist10deg
# Toggle Widgets
elif event.GetKeyCode() == 49: # 1
widgets = [self.modeText,self.wpText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 50: # 2
widgets = [self.batOutRec,self.batInRec,self.voltsText,self.ampsText,self.batPerText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 51: # 3
widgets = [self.rollText,self.pitchText,self.yawText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 52: # 4
widgets = [self.airspeedText,self.altitudeText,self.climbRateText]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 53: # 5
widgets = [self.altHistRect,self.altPlot,self.altMarker,self.altText2]
self.toggleWidgets(widgets)
elif event.GetKeyCode() == 54: # 6
widgets = [self.headingTri,self.headingText,self.headingNorthTri,self.headingNorthText,self.headingWPTri,self.headingWPText]
self.toggleWidgets(widgets)
# Update Matplotlib Plot
self.canvas.draw()
self.canvas.Refresh()
self.Refresh()
self.Update()
def encircle(x, y, ax, **kw):
p = np.c_[x, y]
hull = ConvexHull(p)
poly = Polygon(p[hull.vertices, :], **kw)
ax.add_patch(poly)
def boxer(t, xtitle, xvar, ytitle, yvar, ax=None):
if ax is None:
ax = plt.gca()
bp = ax.boxplot(t)
# ax.set_xlabel(xtitle)
ax.set_title(ytitle, size=12)
# ax.set_title()
#ax.spines['top'].set_visible(False)
#ax.spines['right'].set_visible(False)
ax.get_xaxis().tick_bottom()
ax.get_yaxis().tick_left()
ax.tick_params(axis='x', length=0)
ax.tick_params(axis='y', length=0)
# colors = ['m','m','c','c','y','y']
colors = [
'cornflowerblue', 'firebrick', 'cornflowerblue', 'firebrick',
'cornflowerblue', 'firebrick'
]
print len(bp['boxes'])
print len(bp['whiskers'])
print len(bp['fliers'])
for i in range(0, len(bp['boxes'])):
bp['boxes'][i].set_color(colors[i])
# we have two whiskers!
bp['whiskers'][i * 2].set_color(colors[i])
bp['whiskers'][i * 2 + 1].set_color(colors[i])
bp['whiskers'][i * 2].set_linewidth(2)
bp['whiskers'][i * 2 + 1].set_linewidth(2)
# top and bottom fliers
# (set allows us to set many parameters at once)
bp['fliers'][i].set(markerfacecolor=colors[i],
marker='o',
alpha=0.75,
markersize=2,
markeredgecolor='none')
# bp['fliers'][i * 2 + 1].set(markerfacecolor=colors[i],
# marker='o', alpha=0.75, markersize=6,
# markeredgecolor='none')
bp['medians'][i].set_color('black')
bp['medians'][i].set_linewidth(1)
# and 4 caps to remove
for c in bp['caps']:
c.set_linewidth(0)
box = bp['boxes'][i]
box.set_linewidth(0)
boxX = []
boxY = []
for j in range(5):
boxX.append(box.get_xdata()[j])
boxY.append(box.get_ydata()[j])
boxCoords = zip(boxX, boxY)
boxPolygon = Polygon(boxCoords, facecolor=colors[i], linewidth=0)
ax.add_patch(boxPolygon)
plt.xticks([1.5, 3.5, 5.5], ['Aug11', 'Aug17', 'Feb23'], size=9)
plt.yticks(size=10)
# plt.savefig('boxplot_'+yvar+'_sortedby_'+xvar+'.png')
return bp
def plot_mocalum_setup(lidar_id, bbox_id, mc_obj):
"""
Plots 2D geometry of lidar scan and flow field box
Parameters
----------
mc_obj : mocalum
Instance of mocalum class
"""
avg_azimuth = mc_obj.data.meas_cfg[lidar_id]['config']['az'].mean()
no_los = mc_obj.data.meas_cfg[lidar_id]['config']['no_los']
arc_cords = mc_obj._get_prob_cords(lidar_id)[:no_los]
arc_bbox = bbox_pts_from_array(
mc_obj._get_prob_cords(lidar_id)[:,
(0, 1)].dot(_rot_matrix(avg_azimuth)))
arc_bbox = arc_bbox.dot(np.linalg.inv(_rot_matrix(avg_azimuth)))
x_len = arc_bbox[:, 0].max() - arc_bbox[:, 0].min()
y_len = arc_bbox[:, 1].max() - arc_bbox[:, 0].min()
diagonal = ((x_len)**2 + (y_len)**2)**(.5)
lidar_pos = mc_obj.data.meas_cfg[lidar_id]['position']
wind_dir = mc_obj.data.fmodel_cfg['wind_from_direction']
R_tb = mc_obj.data.ffield_bbox_cfg[bbox_id]['CRS']['rot_matrix']
bbox_pts = bbox_pts_from_cfg(mc_obj.data.ffield_bbox_cfg[bbox_id])
min_bbox_pts = bbox_pts.dot(inv(R_tb))
bbox_c = min_bbox_pts.mean(axis=0)
wind_dir_pt = spher2cart(wind_dir, 0, diagonal / 4)[:2]
flowbox = Polygon(min_bbox_pts,
alpha=0.4,
color='grey',
label="flow field bbox")
arcbox = Polygon(arc_bbox,
alpha=0.4,
color='blue',
label="measurements bbox")
fig, ax = plt.subplots(figsize=(7.5, 7.5))
plt.grid()
# plt.scatter(bbox_pts[:,0],bbox_pts[:,1], c="black")
plt.scatter(arc_cords[:, 0],
arc_cords[:, 1],
c="blue",
label='measurements',
zorder=10)
plt.arrow(bbox_c[0],
bbox_c[1],
-wind_dir_pt[0],
-wind_dir_pt[1],
width=8,
color="red",
label='wind',
zorder=50)
for i, pt in enumerate(arc_cords):
if i == 0:
plt.plot([lidar_pos[0], pt[0]], [lidar_pos[1], pt[1]],
c='black',
alpha=0.4,
label='beam')
else:
plt.plot([lidar_pos[0], pt[0]], [lidar_pos[1], pt[1]],
c='black',
alpha=0.4)
# plt.scatter(tbox_pts[:,0], tbox_pts[:,1], c="red")
plt.scatter(lidar_pos[0],
lidar_pos[1],
c="green",
label='lidar',
zorder=150)
ax.add_patch(flowbox)
ax.add_patch(arcbox)
ax.set_aspect('equal')
plt.legend(loc="upper left")
plt.show()
