mirror of
https://github.com/Team4388/autoPlanner2025.git
synced 2026-06-09 00:38:05 -06:00
Daniel Carta
99a298ec9b
- Added an about page that shows readme - Added even more comments - Added a playback feature - 100x the pain and suffering!
361 lines
15 KiB
Python
361 lines
15 KiB
Python
import sys
|
|
import os
|
|
import numpy as np
|
|
from PySide6.QtWidgets import QApplication, QLabel, QMainWindow, QPushButton, QVBoxLayout, QHBoxLayout, QWidget, QMessageBox
|
|
from PySide6.QtGui import QPixmap, QMouseEvent, QPainter, QPen, QColor, QPainterPath, QPolygon, QFont, QKeyEvent
|
|
from PySide6.QtCore import Qt, QPoint, QRect
|
|
|
|
from buttonEditor import ButtonEditor # Import the button editor
|
|
from about import AboutWindow
|
|
|
|
'''
|
|
This is the path planner window
|
|
The path planner lets the user add nodes and control points to let them change the bezier curves and the path of the robot during the auto
|
|
'''
|
|
|
|
class PathPlanner(QMainWindow):
|
|
def __init__(self):
|
|
super().__init__()
|
|
self.setWindowTitle("Path Planner") # Set the window title
|
|
|
|
# Set up the arrays
|
|
self.coordinates = np.empty((0, 2), dtype=int) # Make an empty array for the coordinates of objects
|
|
self.controlPoints = []
|
|
self.rotationHandles = []
|
|
self.nodeAngles = []
|
|
|
|
# Find the field png and then set the background to that png
|
|
self.imageLabel = QLabel(self)
|
|
scriptDir = os.path.dirname(os.path.abspath(__file__))
|
|
imagePath = os.path.join(scriptDir, "images", "Field.png")
|
|
self.pixmap = QPixmap(imagePath)
|
|
|
|
if self.pixmap.isNull():
|
|
self.imageLabel.setText(f"Image not found at: {imagePath}")
|
|
else:
|
|
self.imageLabel.setPixmap(self.pixmap)
|
|
|
|
# Buttons at the top of the screen
|
|
self.mainWindowButton = QPushButton("Main Window")
|
|
self.mainWindowButton.clicked.connect(self.showMainWindow)
|
|
self.buttonEditorButton = QPushButton("Button Editor")
|
|
self.buttonEditorButton.clicked.connect(self.showButtonEditor)
|
|
self.aboutButton = QPushButton("About")
|
|
self.aboutButton.clicked.connect(self.showAbout)
|
|
|
|
# Button layouts
|
|
buttonLayout = QHBoxLayout()
|
|
buttonLayout.addWidget(self.mainWindowButton)
|
|
buttonLayout.addWidget(self.buttonEditorButton)
|
|
buttonLayout.addWidget(self.aboutButton)
|
|
|
|
# Adding the button layout to the main layout
|
|
layout = QVBoxLayout()
|
|
layout.addLayout(buttonLayout)
|
|
layout.addWidget(self.imageLabel)
|
|
|
|
# Set the button editor to that script
|
|
self.buttonEditor = ButtonEditor(self)
|
|
|
|
# Defining the overall layout
|
|
container = QWidget()
|
|
container.setLayout(layout)
|
|
self.setCentralWidget(container)
|
|
|
|
# Resize the window to all of the layouts and widgets added
|
|
self.resize(self.pixmap.width(), self.pixmap.height() + 60)
|
|
|
|
# Let the app track the users mouse
|
|
self.setMouseTracking(True)
|
|
self.lastClickPos = QPoint()
|
|
|
|
# Variables
|
|
self.coordinates = np.empty((0, 2), dtype=int) # Define the coordinate array again
|
|
self.lastClickTime = 0 # The last time the user clicked on the sceen
|
|
self.nodeSize = 35 # How large to make the nodes
|
|
self.handleSize = 15 # How large to make the rotation/control handles
|
|
self.rotationHandleDistance = 35 # How far the rotation handles are from the node
|
|
self.controlPoints = [] # Stores all of the control points
|
|
self.rotationHandles = [] # Stores all of the rotation handles
|
|
self.nodeAngles = [] # Stores all of the node angles
|
|
|
|
# Tell what the user is currently dragging
|
|
self.draggingControlPoint = False
|
|
self.draggingNode = False
|
|
self.draggingRotationHandle = False
|
|
self.draggingControlPointIndex = (-1, -1)
|
|
self.draggingNodeIndex = -1
|
|
self.draggingRotationHandleIndex = -1
|
|
|
|
# Tell when the user presses a key down
|
|
def keyPressEvent(self, event: QKeyEvent):
|
|
if event.key() == Qt.Key_R:
|
|
# Clear the auto
|
|
self.showClearWarning()
|
|
|
|
|
|
|
|
# Tell when the user presses a mouse button
|
|
def mousePressEvent(self, event: QMouseEvent):
|
|
# Get the position of the mouse click
|
|
pos = self.imageLabel.mapFrom(self, event.position().toPoint())
|
|
x, y = pos.x(), pos.y()
|
|
|
|
# Tell if the mouse was click ed in the window
|
|
if 0 <= x < self.pixmap.width() and 0 <= y < self.pixmap.height():
|
|
# If right clicked, add a node
|
|
if event.button() == Qt.RightButton:
|
|
self.coordinates = np.vstack((self.coordinates, [x, y]))
|
|
self.nodeAngles.append(0)
|
|
if len(self.coordinates) >= 2:
|
|
self.calculateControlPoints()
|
|
self.calculateRotationHandlePos()
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
elif event.button() == Qt.LeftButton:
|
|
currentTime = event.timestamp()
|
|
if (currentTime - self.lastClickTime < 300 and
|
|
(pos - self.lastClickPos).manhattanLength() < 5):
|
|
nodeIndex = self.isPointInNode(x, y)
|
|
if nodeIndex != -1:
|
|
self.deleteNode(nodeIndex)
|
|
else:
|
|
controlPointIndex = self.isPointInControlPoint(x, y)
|
|
if controlPointIndex != (-1, -1):
|
|
self.smoothPoints(controlPointIndex[0], controlPointIndex[1])
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
else:
|
|
controlPointIndex = self.isPointInControlPoint(x, y)
|
|
if controlPointIndex != (-1, -1):
|
|
self.draggingControlPoint = True
|
|
self.draggingControlPointIndex = controlPointIndex
|
|
else:
|
|
nodeIndex = self.isPointInNode(x, y)
|
|
if nodeIndex != -1:
|
|
self.draggingNode = True
|
|
self.draggingNodeIndex = nodeIndex
|
|
else:
|
|
rotationHandleIndex = self.isPointInRotationHandle(x, y)
|
|
if rotationHandleIndex != -1:
|
|
self.draggingRotationHandle = True
|
|
self.draggingRotationHandleIndex = rotationHandleIndex
|
|
|
|
self.lastClickTime = currentTime
|
|
self.lastClickPos = pos
|
|
|
|
def deleteNode(self, index):
|
|
self.coordinates = np.delete(self.coordinates, index, axis=0)
|
|
if index < len(self.controlPoints):
|
|
self.controlPoints.pop(index)
|
|
if index > 0 and index < len(self.controlPoints):
|
|
self.controlPoints.pop(index - 1)
|
|
self.nodeAngles.pop(index)
|
|
self.calculateControlPoints()
|
|
self.calculateRotationHandlePos()
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
|
|
def mouseMoveEvent(self, event: QMouseEvent):
|
|
pos = self.imageLabel.mapFrom(self, event.position().toPoint())
|
|
x, y = pos.x(), pos.y()
|
|
|
|
if self.draggingControlPoint:
|
|
self.controlPoints[self.draggingControlPointIndex[0]][self.draggingControlPointIndex[1]] = QPoint(x, y)
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
elif self.draggingNode:
|
|
self.coordinates[self.draggingNodeIndex] = [x, y]
|
|
self.calculateControlPoints()
|
|
self.calculateRotationHandlePos()
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
elif self.draggingRotationHandle:
|
|
nodePos = QPoint(self.coordinates[self.draggingRotationHandleIndex][0],
|
|
self.coordinates[self.draggingRotationHandleIndex][1])
|
|
angle = (np.degrees(np.arctan2(y - nodePos.y(), x - nodePos.x())) + 90) % 360
|
|
self.nodeAngles[self.draggingRotationHandleIndex] = angle
|
|
self.calculateRotationHandlePos()
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
|
|
def mouseReleaseEvent(self, event: QMouseEvent):
|
|
if event.button() == Qt.LeftButton:
|
|
self.draggingControlPoint = False
|
|
self.draggingControlPointIndex = (-1, -1)
|
|
self.draggingNode = False
|
|
self.draggingNodeIndex = -1
|
|
self.draggingRotationHandle = False
|
|
self.draggingRotationHandleIndex = -1
|
|
self.buttonEditor.updateScene()
|
|
|
|
def isPointInRotationHandle(self, x, y):
|
|
for i, handle in enumerate(self.rotationHandles):
|
|
if (abs(x - handle.x()) <= self.handleSize // 2 and
|
|
abs(y - handle.y()) <= self.handleSize // 2):
|
|
return i
|
|
return -1
|
|
|
|
def isPointInControlPoint(self, x, y):
|
|
for i, controlPair in enumerate(self.controlPoints):
|
|
for j, controlPoint in enumerate(controlPair):
|
|
if (abs(x - controlPoint.x()) <= self.handleSize // 2 and
|
|
abs(y - controlPoint.y()) <= self.handleSize // 2):
|
|
return (i, j)
|
|
return (-1, -1)
|
|
|
|
def isPointInNode(self, x, y):
|
|
for i, (nodeX, nodeY) in enumerate(self.coordinates):
|
|
if (abs(x - nodeX) <= self.nodeSize // 2 and
|
|
abs(y - nodeY) <= self.nodeSize // 2):
|
|
return i
|
|
return -1
|
|
|
|
def calculateControlPoints(self):
|
|
if len(self.coordinates) >= 2:
|
|
x1, y1 = self.coordinates[-2]
|
|
x2, y2 = self.coordinates[-1]
|
|
midX, midY = (x1 + x2) // 2, (y1 + y2) // 2
|
|
self.controlPoints.append([
|
|
QPoint(midX - (x2 - x1) // 4, midY - (y2 - y1) // 4),
|
|
QPoint(midX + (x2 - x1) // 4, midY + (y2 - y1) // 4)
|
|
])
|
|
|
|
def calculateRotationHandlePos(self):
|
|
self.rotationHandles = []
|
|
for i, (x, y) in enumerate(self.coordinates):
|
|
angle = self.nodeAngles[i] if i < len(self.nodeAngles) else 0
|
|
radians = np.radians(angle)
|
|
handle_x = x + self.rotationHandleDistance * np.sin(radians)
|
|
handle_y = y - self.rotationHandleDistance * np.cos(radians)
|
|
self.rotationHandles.append(QPoint(int(handle_x), int(handle_y)))
|
|
|
|
def updateScene(self):
|
|
self.buttonEditor.updateScene(self.coordinates, self.controlPoints)
|
|
|
|
def drawScene(self):
|
|
self.pixmap = QPixmap(os.path.join(os.path.dirname(os.path.abspath(__file__)), "images", "Field.png"))
|
|
painter = QPainter(self.pixmap)
|
|
|
|
greyPen = QPen(QColor(127, 127, 127))
|
|
greyPen.setWidth(2)
|
|
painter.setPen(greyPen)
|
|
for i, controlPair in enumerate(self.controlPoints):
|
|
if i < len(self.coordinates) - 1:
|
|
start = QPoint(self.coordinates[i][0], self.coordinates[i][1])
|
|
end = QPoint(self.coordinates[i + 1][0], self.coordinates[i + 1][1])
|
|
|
|
pen = QPen(Qt.yellow)
|
|
pen.setWidth(2)
|
|
painter.setPen(pen)
|
|
|
|
path = QPainterPath()
|
|
path.moveTo(start)
|
|
path.cubicTo(controlPair[0], controlPair[1], end)
|
|
painter.drawPath(path)
|
|
|
|
painter.setBrush(Qt.NoBrush)
|
|
for controlPoint in controlPair:
|
|
painter.setPen(QPen(QColor(127, 127, 127), 1, Qt.DashLine))
|
|
painter.drawLine(start, controlPoint)
|
|
painter.drawLine(end, controlPoint)
|
|
|
|
painter.setPen(Qt.NoPen)
|
|
painter.setBrush(QColor(0, 255, 255))
|
|
for controlPoint in controlPair:
|
|
painter.drawEllipse(
|
|
controlPoint.x() - self.handleSize // 2,
|
|
controlPoint.y() - self.handleSize // 2,
|
|
self.handleSize,
|
|
self.handleSize
|
|
)
|
|
painter.setBrush(Qt.NoBrush)
|
|
|
|
for i, (x, y) in enumerate(self.coordinates):
|
|
if i < len(self.rotationHandles):
|
|
angle = self.nodeAngles[i] if i < len(self.nodeAngles) else 0
|
|
|
|
painter.save()
|
|
painter.translate(x, y)
|
|
painter.rotate(angle)
|
|
|
|
pen = QPen(QColor(127, 127, 127))
|
|
pen.setWidth(2)
|
|
painter.setPen(pen)
|
|
painter.setBrush(Qt.NoBrush)
|
|
painter.drawRect(-self.nodeSize // 2, -self.nodeSize // 2,
|
|
self.nodeSize, self.nodeSize)
|
|
|
|
painter.restore()
|
|
|
|
painter.setPen(Qt.white)
|
|
font = painter.font()
|
|
font.setPointSize(25)
|
|
painter.setFont(font)
|
|
painter.drawText(QRect(x - self.nodeSize // 2, y - self.nodeSize // 2,
|
|
self.nodeSize, self.nodeSize),
|
|
Qt.AlignCenter, str(i + 1))
|
|
|
|
for i, handle in enumerate(self.rotationHandles):
|
|
painter.setBrush(QColor(255, 0, 255))
|
|
painter.drawEllipse(
|
|
handle,
|
|
self.handleSize // 2,
|
|
self.handleSize // 2
|
|
)
|
|
painter.setBrush(Qt.NoBrush)
|
|
|
|
self.imageLabel.setPixmap(self.pixmap)
|
|
self.buttonEditor.updateScene()
|
|
|
|
def smoothPoints(self, curveIndex: int, pointIndex: int):
|
|
if curveIndex + 1 < len(self.controlPoints):
|
|
prevControlPair = self.controlPoints[curveIndex]
|
|
nextControlPair = self.controlPoints[curveIndex + 1]
|
|
node = QPoint(self.coordinates[curveIndex + 1][0], self.coordinates[curveIndex + 1][1])
|
|
|
|
newControl2 = QPoint(
|
|
2 * node.x() - nextControlPair[0].x(),
|
|
2 * node.y() - nextControlPair[0].y()
|
|
)
|
|
self.controlPoints[curveIndex][1] = newControl2
|
|
|
|
newControl1 = QPoint(
|
|
2 * node.x() - prevControlPair[1].x(),
|
|
2 * node.y() - prevControlPair[1].y()
|
|
)
|
|
self.controlPoints[curveIndex + 1][0] = newControl1
|
|
|
|
def clearPoints(self):
|
|
self.coordinates = np.empty((0, 2), dtype=int)
|
|
self.controlPoints.clear()
|
|
self.rotationHandles.clear()
|
|
self.nodeAngles.clear()
|
|
self.drawScene()
|
|
self.buttonEditor.updateScene()
|
|
|
|
def showClearWarning(self):
|
|
reply = QMessageBox.question(self, "Clear Auto",
|
|
"Are you sure you want to clear this auto?",
|
|
QMessageBox.Yes | QMessageBox.No, QMessageBox.No)
|
|
if reply == QMessageBox.Yes:
|
|
self.clearPoints()
|
|
|
|
def showMainWindow(self):
|
|
self.show()
|
|
self.buttonEditor.hide()
|
|
|
|
def showButtonEditor(self):
|
|
self.hide()
|
|
self.buttonEditor.show()
|
|
|
|
def showAbout(self):
|
|
self.about_window = AboutWindow()
|
|
self.about_window.show()
|
|
|
|
if __name__ == "__main__":
|
|
app = QApplication(sys.argv)
|
|
window = PathPlanner()
|
|
window.show()
|
|
sys.exit(app.exec())
|