def atualizar_setor(setor: Wedge, abertura_min: float) -> [Wedge]:
"""Atualiza os valores de theta e opacidade do setor, movimentando-o"""
global glob_multip_alpha
# O valor de theta = valor atual + incremento (Se a soma não ultrapassar 360)
# Se a soma ultrapassar 360, o valor será o resto da divisão (soma / 360)
setor.set_theta1((setor.theta1 + glob_increm_ang) % 360)
setor.set_theta2(setor.theta1 + abertura_min)
# Se a opacidade do setor for maior que 0.4 ou for menor que o valor mínimo:
# inverta de incremento para decremento ou vice-versa
if setor.get_alpha() >= 0.4 or setor.get_alpha() < glob_incremento_alpha:
glob_multip_alpha = (-1) * glob_multip_alpha
setor.set_alpha(setor.get_alpha() +
glob_incremento_alpha * glob_multip_alpha)
return [setor]
class ArtificialHorizonArtist(Artist):
def __init__(self, ax: plt.Axes, attitude_data: SO3):
if not isinstance(ax, plt.Axes):
raise TypeError("The axes must be of plt.Axes type.")
if not isinstance(attitude_data, SO3):
raise TypeError("The attitude_data must be an SO3 element.")
# Background
ah_circle = Circle((0, 0), radius=1.0, color='w', ec='w')
ah_colouring = Circle((0, 0), radius=5.0, color='k')
ax.add_patch(ah_colouring)
ax.add_patch(ah_circle)
# Horizon line
self._attitude_data = attitude_data
true_horizon = self._compute_horizon(attitude_data)
self._horizon_line, = ax.plot(true_horizon[0, :], true_horizon[1, :])
# Horizon shading
height = self._compute_horizon_height(attitude_data)
slope = self._compute_horizon_slope(attitude_data) * 180.0 / np.pi
self._shade = Wedge((0, height), 2.0, -180 + slope, slope, alpha=0.5)
ax.add_patch(self._shade)
# Clip path
clip_circle = Circle((0, 0), radius=1.0, transform=ax.transData)
self._horizon_line.set_clip_path(clip_circle)
self._shade.set_clip_path(clip_circle)
# Center Overlay
ax.add_patch(Arc((0, 0), 0.3, 0.3, theta1=180, lw=2.0))
ax.plot([0.15, 0.6], [0, 0], color='k', lw=2.0)
ax.plot([-0.15, -0.6], [0, 0], color='k', lw=2.0)
# Axes settings
ax.axis('square')
ax.set_xlim([-1, 1])
ax.set_ylim([-1, 1])
def set_attitude_data(self, attitude: SE3):
if not isinstance(attitude, SO3):
raise TypeError("The attitude_data must be an SO3 element.")
self._attitude_data = attitude
true_horizon = self._compute_horizon(attitude)
self._horizon_line.set_data(true_horizon[0, :], true_horizon[1, :])
height = self._compute_horizon_height(attitude)
slope = self._compute_horizon_slope(attitude) * 180.0 / np.pi
self._shade.set_center((0, height))
self._shade.set_theta1(-180 + slope)
self._shade.set_theta2(slope)
@staticmethod
def _compute_horizon(attitude: SO3):
angles = np.linspace(0, 2 * np.pi, 50)
base_horizon = np.vstack(
(np.cos(angles), np.sin(angles), np.zeros(angles.shape)))
true_horizon = attitude.inv() * base_horizon
true_horizon = true_horizon[:, true_horizon[0, :] > 0]
true_horizon = true_horizon / true_horizon[0, :]
true_horizon = true_horizon[1:, :]
return true_horizon
@staticmethod
def _compute_horizon_slope(attitude: SO3):
theta = ArtificialHorizonArtist._find_heading_angle(attitude)
H = attitude.inv() * np.array([[np.cos(theta)], [np.sin(theta)], [0.0]
])
DH = attitude.inv() * np.array([[-np.sin(theta)], [np.cos(theta)],
[0.0]])
DH2 = H[0, 0] * DH - DH[0, 0] * H
slope = np.arctan2(DH2[2, 0], DH2[1, 0])
return slope
@staticmethod
def _compute_horizon_height(attitude: SO3):
theta = ArtificialHorizonArtist._find_heading_angle(attitude)
H = attitude.inv() * np.array([[np.cos(theta)], [np.sin(theta)], [0.0]
])
H = H / H[0, 0]
return H[2, 0]
@staticmethod
def _find_heading_angle(attitude):
RT = attitude.as_matrix().T
theta = np.arctan2(-RT[1, 0], RT[1, 1])
return theta
class PeakSelector(_SelectorWidget):
"""Draw a Peak as triangle."""
# pylint: disable=too-many-arguments
# pylint: disable=too-many-instance-attributes
# pylint: disable=invalid-name
# pylint: disable=attribute-defined-outside-init
def __init__(self,
ax,
onselect,
minfwhm=None,
minamp=None,
useblit=False,
wedgeprops=None,
onmove_callback=None,
peak_stays=False,
button=None,
limits=None):
_SelectorWidget.__init__(self,
ax,
onselect,
useblit=useblit,
button=button)
if minfwhm is not None or minamp is not None:
raise NotImplementedError
if wedgeprops is None:
wedgeprops = dict(facecolor='red', alpha=0.5, fill=True)
wedgeprops['animated'] = self.useblit
self.wedge = None
self.pressv = None
if limits is None:
limits = (-np.inf, np.inf)
self.limits = limits
self.wedgeprops = wedgeprops
self.onmove_callback = onmove_callback
self.minfwhm = minfwhm
self.minamp = minamp
self.peak_stays = peak_stays
# Needed when dragging out of axes
self.prev = (0, 0)
# Reset canvas so that `new_axes` connects events.
self.canvas = None
self.new_axes(ax)
def new_axes(self, ax):
"""Set SpanSelector to operate on a new Axes"""
self.ax = ax
if self.canvas is not ax.figure.canvas:
if self.canvas is not None:
self.disconnect_events()
self.canvas = ax.figure.canvas
self.connect_default_events()
self.wedge = Wedge((0, 0),
1e10,
0,
0,
visible=False,
**self.wedgeprops)
if self.peak_stays:
self.stay_wedge = Wedge((0, 0),
1e10,
0,
0,
visible=False,
**self.wedgeprops)
self.stay_wedge.set_animated(False)
self.ax.add_patch(self.stay_wedge)
self.ax.add_patch(self.wedge)
self.artists = [self.wedge]
def set_wedgeprops(self, wedgeprops):
"""Custom: set new rectprops."""
self.wedgeprops = wedgeprops
self.new_axes(self.ax)
def set_limits(self, limits):
"""Sets new limits. Peak will only be drawn when press event occurs
inside these x values."""
self.limits = limits
def ignore(self, event):
"""return *True* if *event* should be ignored"""
return _SelectorWidget.ignore(self, event) or not self.visible
def _press(self, event):
"""on button press event"""
x0, y0 = self._get_data(event)
if not self.limits[0] <= x0 <= self.limits[1]:
return True
self.wedge.set_visible(self.visible)
if self.peak_stays:
self.stay_wedge.set_visible(False)
# really force a draw so that the stay rect is not in
# the blit background
if self.useblit:
self.canvas.draw()
self.pressv = (x0, y0)
self.wedge.set_center((x0, y0))
return False
def _release(self, event):
"""on button release event"""
if self.pressv is None:
return True
self.buttonDown = False
self.wedge.set_visible(False)
if self.peak_stays:
self.stay_wedge.set_center(self.wedge.center)
self.stay_wedge.set_radius(self.wedge.r)
self.stay_wedge.set_theta1(self.wedge.theta1)
self.stay_wedge.set_theta2(self.wedge.theta2)
self.stay_wedge.set_visible(True)
self.canvas.draw_idle()
x0, y0, = self.pressv
center = x0
amplitude = y0
x, y = self._get_data(event)
angle = abs(np.arctan((x - x0) / (y - y0)))
self.onselect(center, amplitude, angle)
self.pressv = None
return False
def _onmove(self, event):
"""on motion notify event"""
if self.pressv is None:
return True
x, y = self._get_data(event)
if x is None:
return True
x0, y0, = self.pressv
angle = abs(np.arctan((x - x0) / (y - y0)))
self.wedge.set_theta1(np.rad2deg(-angle) - 90)
self.wedge.set_theta2(np.rad2deg(angle) - 90)
if self.onmove_callback is not None:
center = x0
amplitude = y0
self.onmove_callback(center, amplitude, angle)
self.update()
return False
class MainWindow(QMainWindow):
def __init__(self, parent=None):
QMainWindow.__init__(self, parent)
self.setWindowTitle('Lidar Boom Geometry')
self.initializeValues()
self.createLayout()
self.initializeDrawing()
def initializeValues(self):
self.crestVal = [0.0, 6.5]
self.backAngleVal = 2
self.backHeightVal = 4
self.frontAngleVal = 3
self.frontHeightVal = 1
self.frontDropVal = 1
self.baseDropVal = 1
self.baseOffsetVal = 12
self.baseHeightVal = 4 * 0.3048
self.boomAngleVal = 20
self.boomLengthVal = 44 * 0.3048
self.mountAngleVal = 80
self.hingeAngleVal = 10
self.mountLengthVal = 0.5
self.waterLevelVal = 1
# Cabling variables
self.cableMountHeight = 5 * 0.3048
self.pulleyALocation = 34 * 0.3048
# Weights
self.sensorWeight = 500
def drawBoomEnd(self):
# The boom cut
length = 15
height = 10
boomPts = [[0, 0], [length, 0], [length + height/tan(radians(self.mountAngleVal)), height], [0, height]]
x = [i[0] for i in boomPts]
y = [i[1] for i in boomPts]
self.barAx.cla()
self.barAx.plot(x, y, 'k')
fill = Polygon(boomPts, facecolor='k')
self.barAx.add_patch(fill)
# The angled hinge
hingeLength = sqrt((height/tan(radians(self.mountAngleVal)))**2 + height**2)
angle = self.hingeAngleVal - 90 - (90 - self.mountAngleVal)
hingePts = [boomPts[2], [boomPts[2][0] + hingeLength*cos(radians(angle)),
boomPts[2][1] + hingeLength*sin(radians(angle))]]
xh = [i[0] for i in hingePts]
yh = [i[1] for i in hingePts]
self.barAx.plot(xh, yh, 'k', lw=2)
self.barAx.set_ylim([-2,12])
def initializeDrawing(self):
self.waterLine, = self.ax.plot([0.0, 300.0], [self.waterLevelVal, self.waterLevelVal])
crestDrop = [self.crestVal[0], self.crestVal[1]-self.frontDropVal]
waterPt = [(crestDrop[1]-self.frontHeightVal)/tan(radians(self.frontAngleVal)), self.frontHeightVal]
landPt = [-(self.crestVal[1]-self.backHeightVal)/tan(radians(self.backAngleVal)), self.backHeightVal]
x = [i[0] for i in [landPt, self.crestVal, crestDrop, waterPt]]
y = [i[1] for i in [landPt, self.crestVal, crestDrop, waterPt]]
self.duneLine, = self.ax.plot(x, y)
baseGround = [self.crestVal[0] - self.baseOffsetVal, self.crestVal[1] - self.baseDropVal]
baseTop = [baseGround[0], baseGround[1] + self.baseHeightVal]
boomEnd = [baseTop[0] + self.boomLengthVal*cos(radians(self.boomAngleVal)), baseTop[1] + self.boomLengthVal*sin(radians(self.boomAngleVal))]
self.oceanRange.setText(QString.number(boomEnd[0] + (boomEnd[1]-self.waterLevelVal)/tan(radians(self.mountAngleVal-60)), 'f', 1))
xb = [i[0] for i in [baseGround, baseTop, boomEnd]]
yb = [i[1] for i in [baseGround, baseTop, boomEnd]]
self.boomLine, = self.ax.plot(xb, yb)
straightAngle = self.boomAngleVal - (90-self.mountAngleVal) - 90 + self.hingeAngleVal
self.wedge = Wedge(boomEnd, 300, straightAngle-40, straightAngle+60, alpha=0.5, color='y')
self.ax.add_artist(self.wedge)
angle = -30+self.mountAngleVal+self.hingeAngleVal+self.boomAngleVal
self.oceanRange.setText(QString.number(boomEnd[0] + (boomEnd[1]-self.waterLevelVal)*tan(radians(angle)), 'f', 1))
# Initialize cabling drawing
cableMountBaseTop = [baseTop[0], baseTop[1] + self.cableMountHeight]
pulleyA = [baseTop[0] + self.pulleyALocation*cos(radians(self.boomAngleVal)), baseTop[1] + self.pulleyALocation*sin(radians(self.boomAngleVal))]
xc = [i[0] for i in [baseTop, cableMountBaseTop, pulleyA]]
yc = [i[1] for i in [baseTop, cableMountBaseTop, pulleyA]]
self.calculateCablingForces()
self.cableLine, = self.ax.plot(xc, yc, 'k')
self.fig.subplots_adjust(left=0.1)
def update(self):
# Dune calculations
crestDrop = [self.crestVal[0], self.crestVal[1]-self.frontDropVal]
waterPt = [(crestDrop[1]-self.frontHeightVal)/tan(radians(self.frontAngleVal)), self.frontHeightVal]
landPt = [-(self.crestVal[1]-self.backHeightVal)/tan(radians(self.backAngleVal)), self.backHeightVal]
xDune = [i[0] for i in [landPt, self.crestVal, crestDrop, waterPt]]
yDune = [i[1] for i in [landPt, self.crestVal, crestDrop, waterPt]]
# Base and boom calculations
baseGround = [self.crestVal[0] - self.baseOffsetVal, self.crestVal[1] - self.baseDropVal]
baseTop = [baseGround[0], baseGround[1] + self.baseHeightVal]
boomEnd = [baseTop[0] + self.boomLengthVal*cos(radians(self.boomAngleVal)), baseTop[1] + self.boomLengthVal*sin(radians(self.boomAngleVal))]
xBoom = [i[0] for i in [baseGround, baseTop, boomEnd]]
yBoom = [i[1] for i in [baseGround, baseTop, boomEnd]]
# Update drawing
self.waterLine.set_ydata([self.waterLevelVal, self.waterLevelVal])
self.duneLine.set_xdata(xDune)
self.duneLine.set_ydata(yDune)
self.boomLine.set_xdata(xBoom)
self.boomLine.set_ydata(yBoom)
self.wedge.set_center(boomEnd)
straightAngle = self.boomAngleVal - (90-self.mountAngleVal) - 90 + self.hingeAngleVal
self.wedge.set_theta1(straightAngle-40)
self.wedge.set_theta2(straightAngle+60)
self.drawBoomEnd()
# Calculate and draw cabling
cableMountBaseTop = [baseTop[0], baseTop[1] + self.cableMountHeight]
pulleyA = [baseTop[0] + self.pulleyALocation*cos(radians(self.boomAngleVal)), baseTop[1] + self.pulleyALocation*sin(radians(self.boomAngleVal))]
xc = [i[0] for i in [baseTop, cableMountBaseTop, pulleyA]]
yc = [i[1] for i in [baseTop, cableMountBaseTop, pulleyA]]
self.cableLine.set_xdata(xc)
self.cableLine.set_ydata(yc)
self.calculateCablingForces()
# Update the interface
angle = -30+self.mountAngleVal+self.hingeAngleVal+self.boomAngleVal
self.oceanRange.setText(QString.number(boomEnd[0] + (boomEnd[1]-self.waterLevelVal)*tan(radians(angle)), 'f', 1))
# Update the canvas
self.fig.canvas.draw()
def calculateCablingForces(self):
h = self.pulleyALocation*sin(radians(self.boomAngleVal)) - self.cableMountHeight
l = self.pulleyALocation*cos(radians(self.boomAngleVal))
innerAngle = abs(atan2(h,l) - radians(self.boomAngleVal))
f = (self.sensorWeight * sin(radians(90 - self.boomAngleVal)) / sin(innerAngle))
self.pulleyAForce.setText(QString.number(f, 'f', 1))
print degrees(innerAngle)
def updateCrest(self, val):
self.crestVal[1] = val
self.update()
def updateWaterLevel(self, val):
self.waterLevelVal = val
self.update()
def updateBaseDrop(self, val):
self.baseDropVal = val
self.update()
def updateBaseOffset(self, val):
self.baseOffsetVal = val
self.update()
def updateBaseHeight(self, val):
self.baseHeightVal = val
self.update()
def updateBoomAngle(self, val):
self.boomAngleVal = val
self.update()
def updateBoomLength(self, val):
self.boomLengthVal = val
self.update()
def updateMountAngle(self, val):
self.mountAngleVal = val
self.update()
def updateHingeAngle(self, val):
self.hingeAngleVal = val
self.update()
def updateCableTowerHeight(self, val):
self.cableMountHeight = val
self.update()
def updatePulleyALocation(self, val):
self.pulleyALocation = val
self.update()
def updateSensorWeight(self, val):
self.sensorWeight = val
self.update()
def createLayout(self):
self.mainFrame = QWidget()
# Main horizontal layout
mainLayout = QHBoxLayout()
# Layouts
controlLayout = QGridLayout()
infoLayout = QGridLayout()
leftLayout = QVBoxLayout()
viewLayout = QVBoxLayout()
line = QFrame(self.mainFrame)
line.setFrameStyle(QFrame.HLine | QFrame.Raised)
leftLayout.addLayout(controlLayout)
leftLayout.addWidget(line)
leftLayout.addLayout(infoLayout)
leftLayout.addStretch()
mainLayout.addLayout(leftLayout)
mainLayout.addLayout(viewLayout)
# Info List
infoLayout.addWidget(QLabel('<b>LIDAR</b>'), 0, 0, 1, 2, Qt.AlignCenter)
infoLayout.addWidget(QLabel('Ocean Sight Range:'), 1, 0)
infoLayout.addWidget(QLabel('<b>Structural</b>'), 2, 0, 1, 2, Qt.AlignCenter)
infoLayout.addWidget(QLabel('Force - Pulley A (lb)'), 3, 0)
self.oceanRange = QLabel('-', self.mainFrame)
self.pulleyAForce = QLabel('-', self.mainFrame)
infoLayout.addWidget(self.oceanRange, 1, 1)
infoLayout.addWidget(self.pulleyAForce, 3, 1)
infoLayout.setColumnMinimumWidth(1, 100)
# Control grid
controlLayout.addWidget(QLabel('<b>Dune/Ocean</b>'), 0, 0, 1, 2, Qt.AlignCenter)
controlLayout.addWidget(QLabel('Dune Crest'), 1, 0)
controlLayout.addWidget(QLabel('Water Level'), 2, 0)
controlLayout.addWidget(QLabel('<b>Boom Geometry</b>'), 3, 0, 1, 2, Qt.AlignCenter)
controlLayout.addWidget(QLabel('Base Drop'), 4, 0)
controlLayout.addWidget(QLabel('Base Offset'), 5, 0)
controlLayout.addWidget(QLabel('Base Height'), 6, 0)
controlLayout.addWidget(QLabel('Boom Length'), 7, 0)
controlLayout.addWidget(QLabel('Boom Angle'), 8, 0)
controlLayout.addWidget(QLabel('<b>Mount Geometry</b>'), 9, 0, 1, 2, Qt.AlignCenter)
controlLayout.addWidget(QLabel('Mount Cut Angle'), 10, 0)
controlLayout.addWidget(QLabel('Hinge Angle'), 11, 0)
controlLayout.addWidget(QLabel('<b>Cabling Geometry</b>'), 12, 0, 1, 2, Qt.AlignCenter)
controlLayout.addWidget(QLabel('Cable Mount Height'), 13, 0)
controlLayout.addWidget(QLabel('Pulley A Location'), 14, 0)
controlLayout.addWidget(QLabel('<b>Loading (lbs)</b>'), 15, 0, 1, 2, Qt.AlignCenter)
controlLayout.addWidget(QLabel('LIDAR Weight'), 16, 0)
controlLayout.addWidget(QLabel('Boom Weight'), 17, 0)
self.crestHeight = QDoubleSpinBox(self.mainFrame)
self.crestHeight.setSingleStep(0.1)
self.crestHeight.setValue(self.crestVal[1])
controlLayout.addWidget(self.crestHeight, 1, 1)
self.connect(self.crestHeight, SIGNAL('valueChanged(double)'), self.updateCrest)
self.waterLevel = QDoubleSpinBox(self.mainFrame)
self.waterLevel.setSingleStep(0.1)
self.waterLevel.setValue(self.waterLevelVal)
controlLayout.addWidget(self.waterLevel, 2, 1)
self.connect(self.waterLevel, SIGNAL('valueChanged(double)'), self.updateWaterLevel)
self.baseDrop = QDoubleSpinBox(self.mainFrame)
self.baseDrop.setSingleStep(0.1)
self.baseDrop.setValue(self.baseDropVal)
controlLayout.addWidget(self.baseDrop, 4, 1)
self.connect(self.baseDrop, SIGNAL('valueChanged(double)'), self.updateBaseDrop)
self.baseOffset = QDoubleSpinBox(self.mainFrame)
self.baseOffset.setSingleStep(0.1)
self.baseOffset.setValue(self.baseOffsetVal)
controlLayout.addWidget(self.baseOffset, 5, 1)
self.connect(self.baseOffset, SIGNAL('valueChanged(double)'), self.updateBaseOffset)
self.baseHeight = QDoubleSpinBox(self.mainFrame)
self.baseHeight.setSingleStep(0.1)
self.baseHeight.setValue(self.baseHeightVal)
controlLayout.addWidget(self.baseHeight, 6, 1)
self.connect(self.baseHeight, SIGNAL('valueChanged(double)'), self.updateBaseHeight)
self.boomLength = QDoubleSpinBox(self.mainFrame)
self.boomLength.setSingleStep(0.1)
self.boomLength.setValue(self.boomLengthVal)
controlLayout.addWidget(self.boomLength, 7, 1)
self.connect(self.boomLength, SIGNAL('valueChanged(double)'), self.updateBoomLength)
self.boomAngle = QDoubleSpinBox(self.mainFrame)
self.boomAngle.setSingleStep(0.5)
self.boomAngle.setMaximum(360)
self.boomAngle.setValue(self.boomAngleVal)
controlLayout.addWidget(self.boomAngle, 8, 1)
self.connect(self.boomAngle, SIGNAL('valueChanged(double)'), self.updateBoomAngle)
self.mountAngle = QDoubleSpinBox(self.mainFrame)
self.mountAngle.setSingleStep(0.5)
self.mountAngle.setMaximum(360)
self.mountAngle.setValue(self.mountAngleVal)
controlLayout.addWidget(self.mountAngle, 10, 1)
self.connect(self.mountAngle, SIGNAL('valueChanged(double)'), self.updateMountAngle)
self.hingeAngle = QDoubleSpinBox(self.mainFrame)
self.hingeAngle.setSingleStep(0.5)
self.hingeAngle.setMaximum(360)
self.hingeAngle.setValue(self.hingeAngleVal)
controlLayout.addWidget(self.hingeAngle, 11, 1)
self.connect(self.hingeAngle, SIGNAL('valueChanged(double)'), self.updateHingeAngle)
self.cableTowerHeight = QDoubleSpinBox(self.mainFrame)
self.cableTowerHeight.setSingleStep(0.1)
self.cableTowerHeight.setValue(self.cableMountHeight)
controlLayout.addWidget(self.cableTowerHeight, 13, 1)
self.connect(self.cableTowerHeight, SIGNAL('valueChanged(double)'), self.updateCableTowerHeight)
self.pulleyADistance = QDoubleSpinBox(self.mainFrame)
self.pulleyADistance.setSingleStep(0.1)
self.pulleyADistance.setValue(self.pulleyALocation)
controlLayout.addWidget(self.pulleyADistance, 14, 1)
self.connect(self.pulleyADistance, SIGNAL('valueChanged(double)'), self.updatePulleyALocation)
self.lidarWeight = QDoubleSpinBox(self.mainFrame)
self.lidarWeight.setSingleStep(5)
self.lidarWeight.setMaximum(9999999)
self.lidarWeight.setValue(self.sensorWeight)
controlLayout.addWidget(self.lidarWeight, 16, 1)
self.connect(self.lidarWeight, SIGNAL('valueChanged(double)'), self.updateSensorWeight)
# Create the axes
self.fig = Figure((5.0, 4.0))
self.canvas = FigureCanvas(self.fig)
self.canvas.setParent(self.mainFrame)
self.ax = self.fig.add_subplot(211, aspect='equal')
self.barAx = self.fig.add_subplot(212, aspect='equal')
self.ax.set_xlim([-15,10])
self.drawBoomEnd()
self.toolBar = NavigationToolbar(self.canvas, self.mainFrame)
viewLayout.addWidget(self.canvas)
viewLayout.addWidget(self.toolBar)
# Set the main layout
self.mainFrame.setLayout(mainLayout)
self.setCentralWidget(self.mainFrame)