mirror of
https://github.com/Team4388/autoPlanner2025.git
synced 2026-06-09 00:38:05 -06:00
Le risque
Added most of the functionality to the button editor
This commit is contained in:
Daniel Carta
@@ -16,9 +16,13 @@ python3 ./main.py
|
||||
##### "Path Editor" Tab:
|
||||
|
||||
- Right click to add nodes
|
||||
- Left click on specific points to manipulate paths and nodes
|
||||
- Left click on specific points to manipulate paths and nodes:
|
||||
- Drag cyan circles (Control points) to change the shape of the path
|
||||
- Drag magenta nodes change the rotation of the robot
|
||||
- Drag white numbers (Nodes) to change the position of the robot
|
||||
- Double click on nodes to delete them
|
||||
- Double click on control points to make path, and robot movment continuous, while keeping the node clicked the at the same location (Smooth the path)
|
||||
- Press the 'r' key to delete the whole auto
|
||||
|
||||
##### "Button editor" Tab:
|
||||
|
||||
@@ -34,5 +38,5 @@ python3 ./main.py
|
||||
|
||||
### Known Bugs:
|
||||
|
||||
- Smoothing function is janky (I want to completely redo it)
|
||||
- Smoothing function is janky sometimes
|
||||
- Sometimes the control points spawn in random locations but I cant seem to replicate it?
|
||||
+110
-45
@@ -1,8 +1,8 @@
|
||||
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
|
||||
from PySide6.QtWidgets import QApplication, QLabel, QMainWindow, QPushButton, QVBoxLayout, QHBoxLayout, QWidget, QScrollArea
|
||||
from PySide6.QtGui import QPixmap, QPainter, QPainterPath, QColor, QPen
|
||||
from PySide6.QtCore import Qt, QPoint, QRect
|
||||
|
||||
class ButtonEditor(QMainWindow):
|
||||
@@ -12,7 +12,6 @@ class ButtonEditor(QMainWindow):
|
||||
self.pathPlanner = pathPlanner
|
||||
|
||||
self.imageLabel = QLabel(self)
|
||||
|
||||
scriptDir = os.path.dirname(os.path.abspath(__file__))
|
||||
imagePath = os.path.join(scriptDir, "images", "Field.png")
|
||||
self.pixmap = QPixmap(imagePath)
|
||||
@@ -35,75 +34,141 @@ class ButtonEditor(QMainWindow):
|
||||
layout.addLayout(buttonLayout)
|
||||
layout.addWidget(self.imageLabel)
|
||||
|
||||
self.scrollArea = QScrollArea()
|
||||
self.scrollArea.setWidgetResizable(True)
|
||||
self.scrollArea.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOn)
|
||||
self.scrollArea.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
|
||||
layout.addWidget(self.scrollArea)
|
||||
|
||||
self.rectanglesWidget = QWidget()
|
||||
self.scrollArea.setWidget(self.rectanglesWidget)
|
||||
|
||||
container = QWidget()
|
||||
container.setLayout(layout)
|
||||
self.setCentralWidget(container)
|
||||
|
||||
self.resize(self.pixmap.width(), self.pixmap.height() + 60)
|
||||
self.resize(self.pixmap.width(), self.pixmap.height() + 200)
|
||||
|
||||
# Variables
|
||||
# VariablesssszczadasdadawsadsaaDSdasas
|
||||
self.matchLength = 15
|
||||
self.TPS = 50
|
||||
self.matchTicks = self.matchLength * self.TPS
|
||||
self.displayTickResolution = 4
|
||||
self.displayTickResolution = 18
|
||||
self.displayTicks = round(self.matchTicks / self.displayTickResolution)
|
||||
|
||||
self.keyFrames = []
|
||||
self.displayFrames = []
|
||||
|
||||
def addKeyFrames(self):
|
||||
currentFrame = 0
|
||||
self.setupFrames()
|
||||
|
||||
for i in range(0, self.displayTicks):
|
||||
self.keyFrames.append(currentFrame)
|
||||
def setupFrames(self):
|
||||
self.keyFrames = list(range(1, self.matchTicks + 1))
|
||||
self.displayFrames = list(range(1, self.displayTicks + 1))
|
||||
|
||||
print(len(self.keyFrames))
|
||||
print(len(self.displayFrames))
|
||||
|
||||
def showButtonEditor(self):
|
||||
self.show()
|
||||
self.pathPlanner.hide()
|
||||
if self.pathPlanner:
|
||||
self.pathPlanner.hide()
|
||||
|
||||
def showPathPlanner(self):
|
||||
self.hide()
|
||||
self.pathPlanner.show()
|
||||
if self.pathPlanner:
|
||||
self.pathPlanner.show()
|
||||
|
||||
def updateScene(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)
|
||||
|
||||
# Draw the Bezier curve segments
|
||||
if self.pathPlanner.coordinates.size > 0:
|
||||
for i in range(len(self.pathPlanner.coordinates) - 1):
|
||||
start = QPoint(self.pathPlanner.coordinates[i][0], self.pathPlanner.coordinates[i][1])
|
||||
end = QPoint(self.pathPlanner.coordinates[i + 1][0], self.pathPlanner.coordinates[i + 1][1])
|
||||
|
||||
if i < len(self.pathPlanner.controlPoints):
|
||||
controlPair = self.pathPlanner.controlPoints[i]
|
||||
|
||||
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)
|
||||
|
||||
# Draw the nodes
|
||||
painter.setPen(Qt.white)
|
||||
font = painter.font()
|
||||
font.setPointSize(25)
|
||||
painter.setFont(font)
|
||||
|
||||
painter.setPen(Qt.NoPen)
|
||||
painter.setBrush(Qt.white)
|
||||
|
||||
for i, (x, y) in enumerate(self.pathPlanner.coordinates):
|
||||
nodeRect = QRect(x - self.pathPlanner.nodeSize // 6, y - self.pathPlanner.nodeSize // 6,
|
||||
self.pathPlanner.nodeSize // 3, self.pathPlanner.nodeSize // 3)
|
||||
painter.drawEllipse(nodeRect)
|
||||
painter.drawText(nodeRect, Qt.AlignCenter, str(i + 1))
|
||||
if self.pathPlanner and hasattr(self.pathPlanner, 'coordinates'):
|
||||
for i, (x, y) in enumerate(self.pathPlanner.coordinates):
|
||||
nodeRect = QRect(x - self.pathPlanner.nodeSize // 6, y - self.pathPlanner.nodeSize // 6,
|
||||
self.pathPlanner.nodeSize // 3, self.pathPlanner.nodeSize // 3)
|
||||
painter.drawEllipse(nodeRect)
|
||||
painter.drawText(nodeRect, Qt.AlignCenter, str(i + 1))
|
||||
|
||||
angle = self.pathPlanner.nodeAngles[i] if i < len(self.pathPlanner.nodeAngles) else 0
|
||||
self.drawRobot(painter, QPoint(x, y), angle)
|
||||
|
||||
if len(self.pathPlanner.coordinates) > 1:
|
||||
for i in range(len(self.pathPlanner.coordinates) - 1):
|
||||
start = QPoint(self.pathPlanner.coordinates[i][0], self.pathPlanner.coordinates[i][1])
|
||||
end = QPoint(self.pathPlanner.coordinates[i + 1][0], self.pathPlanner.coordinates[i + 1][1])
|
||||
|
||||
if i < len(self.pathPlanner.controlPoints):
|
||||
controlPair = self.pathPlanner.controlPoints[i]
|
||||
|
||||
start_angle = self.pathPlanner.nodeAngles[i] if i < len(self.pathPlanner.nodeAngles) else 0
|
||||
end_angle = self.pathPlanner.nodeAngles[i + 1] if i + 1 < len(self.pathPlanner.nodeAngles) else 0
|
||||
|
||||
for t in np.linspace(0, 1, self.displayTicks)[1:-1]:
|
||||
point = self.pointOnBezierCurve(start, controlPair[0], controlPair[1], end, t)
|
||||
current_angle = self.interpolateAngle(start_angle, end_angle, t)
|
||||
self.drawRobot(painter, point, current_angle)
|
||||
|
||||
painter.end()
|
||||
self.imageLabel.setPixmap(self.pixmap)
|
||||
|
||||
def interpolateAngle(self, start_angle, end_angle, t):
|
||||
diff = (end_angle - start_angle + 180) % 360 - 180
|
||||
return start_angle + diff * t
|
||||
|
||||
def drawRobot(self, painter, position, angle):
|
||||
side_length = self.pathPlanner.nodeSize
|
||||
half_side = side_length / 2
|
||||
|
||||
painter.save()
|
||||
painter.translate(position)
|
||||
painter.rotate(angle - 90)
|
||||
painter.setBrush(Qt.NoBrush)
|
||||
painter.setPen(QPen(QColor(127, 127, 127), 2))
|
||||
painter.drawRect(-half_side, -half_side, side_length, side_length)
|
||||
|
||||
painter.setPen(QPen(QColor(255, 0, 0), 2))
|
||||
painter.drawLine(0, 0, half_side, 0)
|
||||
painter.drawLine(half_side, 0, half_side - 5, -5)
|
||||
painter.drawLine(half_side, 0, half_side - 5, 5)
|
||||
|
||||
painter.restore()
|
||||
|
||||
def pointOnBezierCurve(self, start, control1, control2, end, t):
|
||||
x = (1-t)**3 * start.x() + 3*(1-t)**2*t * control1.x() + 3*(1-t)*t**2 * control2.x() + t**3 * end.x()
|
||||
y = (1-t)**3 * start.y() + 3*(1-t)**2*t * control1.y() + 3*(1-t)*t**2 * control2.y() + t**3 * end.y()
|
||||
return QPoint(int(x), int(y))
|
||||
|
||||
def updateRectangles(self):
|
||||
if self.rectanglesWidget.layout():
|
||||
QWidget().setLayout(self.rectanglesWidget.layout())
|
||||
|
||||
layout = QHBoxLayout()
|
||||
layout.setSpacing(2)
|
||||
|
||||
rect_width = 20
|
||||
rect_height = 50
|
||||
|
||||
for frame in self.displayFrames:
|
||||
rectWidget = QWidget()
|
||||
rectWidget.setFixedSize(rect_width, rect_height)
|
||||
rectWidget.setAutoFillBackground(True)
|
||||
rectWidget.setStyleSheet("background-color: white; border: 1px solid black;")
|
||||
|
||||
layout.addWidget(rectWidget)
|
||||
|
||||
self.rectanglesWidget.setLayout(layout)
|
||||
self.rectanglesWidget.setFixedHeight(rect_height + 10)
|
||||
self.rectanglesWidget.setMinimumWidth(len(self.displayFrames) * (rect_width + 2))
|
||||
|
||||
self.scrollArea.setWidget(self.rectanglesWidget)
|
||||
self.scrollArea.setFixedHeight(rect_height + 30)
|
||||
|
||||
if __name__ == "__main__":
|
||||
app = QApplication(sys.argv)
|
||||
window = ButtonEditor(None)
|
||||
window.updateScene()
|
||||
window.show()
|
||||
sys.exit(app.exec())
|
||||
@@ -2,7 +2,7 @@ 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
|
||||
from PySide6.QtGui import QPixmap, QMouseEvent, QPainter, QPen, QColor, QPainterPath, QPolygon, QFont, QKeyEvent
|
||||
from PySide6.QtCore import Qt, QPoint, QRect
|
||||
|
||||
from buttonEditor import ButtonEditor
|
||||
@@ -69,6 +69,10 @@ class PathPlanner(QMainWindow):
|
||||
self.draggingNodeIndex = -1
|
||||
self.draggingRotationHandleIndex = -1
|
||||
|
||||
def keyPressEvent(self, event: QKeyEvent):
|
||||
if event.key() == Qt.Key_R:
|
||||
self.showClearWarning()
|
||||
|
||||
def mousePressEvent(self, event: QMouseEvent):
|
||||
pos = self.imageLabel.mapFrom(self, event.position().toPoint())
|
||||
x, y = pos.x(), pos.y()
|
||||
@@ -76,6 +80,7 @@ class PathPlanner(QMainWindow):
|
||||
if 0 <= x < self.pixmap.width() and 0 <= y < self.pixmap.height():
|
||||
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()
|
||||
@@ -116,43 +121,36 @@ class PathPlanner(QMainWindow):
|
||||
def deleteNode(self, index):
|
||||
self.coordinates = np.delete(self.coordinates, index, axis=0)
|
||||
if index < len(self.controlPoints):
|
||||
del self.controlPoints[index]
|
||||
self.controlPoints.pop(index)
|
||||
if index > 0 and index < len(self.controlPoints):
|
||||
del self.controlPoints[index - 1]
|
||||
if index < len(self.rotationHandles):
|
||||
del self.rotationHandles[index]
|
||||
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:
|
||||
pos = self.imageLabel.mapFrom(self, event.position().toPoint())
|
||||
x, y = pos.x(), pos.y()
|
||||
curveIndex, pointIndex = self.draggingControlPointIndex
|
||||
self.controlPoints[curveIndex][pointIndex] = QPoint(x, y)
|
||||
self.controlPoints[self.draggingControlPointIndex[0]][self.draggingControlPointIndex[1]] = QPoint(x, y)
|
||||
self.drawScene()
|
||||
self.buttonEditor.updateScene()
|
||||
elif self.draggingNode:
|
||||
pos = self.imageLabel.mapFrom(self, event.position().toPoint())
|
||||
x, y = pos.x(), pos.y()
|
||||
self.coordinates[self.draggingNodeIndex] = [x, y]
|
||||
self.calculateControlPoints()
|
||||
self.calculateRotationHandlePos()
|
||||
self.drawScene()
|
||||
self.buttonEditor.updateScene()
|
||||
elif self.draggingRotationHandle:
|
||||
pos = self.imageLabel.mapFrom(self, event.position().toPoint())
|
||||
x, y = pos.x(), pos.y()
|
||||
nodeX, nodeY = self.coordinates[self.draggingRotationHandleIndex]
|
||||
dx = x - nodeX
|
||||
dy = y - nodeY
|
||||
angle = np.arctan2(dy, dx)
|
||||
|
||||
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
|
||||
|
||||
rotationHandleX = int(nodeX + self.rotationHandleDistance * np.cos(angle))
|
||||
rotationHandleY = int(nodeY + self.rotationHandleDistance * np.sin(angle))
|
||||
self.rotationHandles[self.draggingRotationHandleIndex] = QPoint(rotationHandleX, rotationHandleY)
|
||||
self.calculateRotationHandlePos()
|
||||
self.drawScene()
|
||||
self.buttonEditor.updateScene()
|
||||
|
||||
@@ -199,19 +197,13 @@ class PathPlanner(QMainWindow):
|
||||
])
|
||||
|
||||
def calculateRotationHandlePos(self):
|
||||
self.rotationHandles = []
|
||||
for i, (x, y) in enumerate(self.coordinates):
|
||||
if i >= len(self.nodeAngles):
|
||||
self.nodeAngles.append(np.pi) # Default angle for new nodes
|
||||
|
||||
angle = self.nodeAngles[i]
|
||||
|
||||
rotationHandleX = int(x + self.rotationHandleDistance * np.cos(angle))
|
||||
rotationHandleY = int(y + self.rotationHandleDistance * np.sin(angle))
|
||||
|
||||
if i >= len(self.rotationHandles):
|
||||
self.rotationHandles.append(QPoint(rotationHandleX, rotationHandleY))
|
||||
else:
|
||||
self.rotationHandles[i] = QPoint(rotationHandleX, rotationHandleY)
|
||||
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)
|
||||
@@ -256,11 +248,11 @@ class PathPlanner(QMainWindow):
|
||||
|
||||
for i, (x, y) in enumerate(self.coordinates):
|
||||
if i < len(self.rotationHandles):
|
||||
angle = self.nodeAngles[i]
|
||||
angle = self.nodeAngles[i] if i < len(self.nodeAngles) else 0
|
||||
|
||||
painter.save()
|
||||
painter.translate(x, y)
|
||||
painter.rotate(np.degrees(angle))
|
||||
painter.rotate(angle)
|
||||
|
||||
pen = QPen(QColor(127, 127, 127))
|
||||
pen.setWidth(2)
|
||||
|
||||
Reference in New Issue
Block a user