diff --git a/README.md b/README.md index 9046cd5..3366f78 100644 --- a/README.md +++ b/README.md @@ -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? \ No newline at end of file diff --git a/buttonEditor.py b/buttonEditor.py index 65c4f5e..1d3c616 100644 --- a/buttonEditor.py +++ b/buttonEditor.py @@ -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()) \ No newline at end of file diff --git a/main.py b/main.py index 3ca80e5..9e00afa 100644 --- a/main.py +++ b/main.py @@ -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)