Team4388/autoPlanner2025
1
0
Fork 0
mirror of https://github.com/Team4388/autoPlanner2025.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Haldern

Browse Source 
- Made it two control points instead of one handle
- Added a warning if you clear the auto
- Added a lot of bugs
This commit is contained in:
Daniel Carta
2024-07-03 13:35:23 -06:00
parent e83f2aa2bf
commit 63bec5aeba
2 changed files with 101 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+1 -2
View File
@@ -18,7 +18,7 @@ python3 ./main.py
- Right click to add nodes - 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
- Double click on nodes to delete them - 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 - 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)
##### "Button editor" Tab: ##### "Button editor" Tab:
@@ -33,4 +33,3 @@ python3 ./main.py
- Click export, and save to a file - Click export, and save to a file
### Known Bugs: ### Known Bugs:
main.py
+100 -90
View File
@@ -1,7 +1,7 @@
import sys import sys
import os import os
import numpy as np import numpy as np
from PySide6.QtWidgets import QApplication, QLabel, QMainWindow, QPushButton, QVBoxLayout, QWidget from PySide6.QtWidgets import QApplication, QLabel, QMainWindow, QPushButton, QVBoxLayout, 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
from PySide6.QtCore import Qt, QPoint, QRect from PySide6.QtCore import Qt, QPoint, QRect
@@ -11,7 +11,7 @@ class MainWindow(QMainWindow):
self.setWindowTitle("Auto Planner") self.setWindowTitle("Auto Planner")
# Set background image to the field #Set background image to the field
self.image_label = QLabel(self) self.image_label = QLabel(self)
script_dir = os.path.dirname(os.path.abspath(__file__)) script_dir = os.path.dirname(os.path.abspath(__file__))
@@ -23,11 +23,11 @@ class MainWindow(QMainWindow):
else: else:
self.image_label.setPixmap(self.pixmap) self.image_label.setPixmap(self.pixmap)
# Buttons #Buttons
self.clear_button = QPushButton("Clear Auto") self.clear_button = QPushButton("Clear Auto")
self.clear_button.clicked.connect(self.clear_points) self.clear_button.clicked.connect(self.show_clear_warning)
# Layout of the auto planner #Layout of the auto planner
layout = QVBoxLayout() layout = QVBoxLayout()
layout.addWidget(self.clear_button) layout.addWidget(self.clear_button)
layout.addWidget(self.image_label) layout.addWidget(self.image_label)
@@ -40,7 +40,7 @@ class MainWindow(QMainWindow):
self.setMouseTracking(True) self.setMouseTracking(True)
# Variables #Variables
self.last_click_pos = QPoint() self.last_click_pos = QPoint()
self.coordinates = np.empty((0, 2), dtype=int) self.coordinates = np.empty((0, 2), dtype=int)
@@ -48,13 +48,13 @@ class MainWindow(QMainWindow):
self.node_size = 35 self.node_size = 35
self.handle_size = 15 self.handle_size = 15
self.rotation_handle_distance = 35 self.rotation_handle_distance = 35
self.handles = [] self.control_points = []
self.rotation_handles = [] self.rotation_handles = []
self.node_angles = [] self.node_angles = []
self.dragging_handle = False self.dragging_control_point = False
self.dragging_node = False self.dragging_node = False
self.dragging_rotation_handle = False self.dragging_rotation_handle = False
self.dragging_handle_index = -1 self.dragging_control_point_index = (-1, -1)
self.dragging_node_index = -1 self.dragging_node_index = -1
self.dragging_rotation_handle_index = -1 self.dragging_rotation_handle_index = -1
@@ -67,7 +67,7 @@ class MainWindow(QMainWindow):
if event.button() == Qt.RightButton: if event.button() == Qt.RightButton:
self.coordinates = np.vstack((self.coordinates, [x, y])) self.coordinates = np.vstack((self.coordinates, [x, y]))
if len(self.coordinates) >= 2: if len(self.coordinates) >= 2:
self.calculate_handle_pos() self.calculate_control_points()
self.calculate_rotation_handle_pos() self.calculate_rotation_handle_pos()
self.draw_scene() self.draw_scene()
self.draw_scene() self.draw_scene()
@@ -79,50 +79,52 @@ class MainWindow(QMainWindow):
if node_index != -1: if node_index != -1:
self.delete_node(node_index) self.delete_node(node_index)
else: else:
handle_index = self.is_point_in_handle(x, y) control_point_index = self.is_point_in_control_point(x, y)
if handle_index != -1: if control_point_index != (-1, -1):
self.smoothPoints(handle_index) self.smoothPoints(control_point_index[0], control_point_index[1])
self.draw_scene() self.draw_scene()
else: else:
handle_index = self.is_point_in_handle(x, y) control_point_index = self.is_point_in_control_point(x, y)
if handle_index != -1: if control_point_index != (-1, -1):
self.dragging_handle = True self.dragging_control_point = True
self.dragging_handle_index = handle_index self.dragging_control_point_index = control_point_index
else:
node_index = self.is_point_in_node(x, y)
if node_index != -1:
self.dragging_node = True
self.dragging_node_index = node_index
else: else:
rotation_handle_index = self.is_point_in_rotation_handle(x, y) node_index = self.is_point_in_node(x, y)
if rotation_handle_index != -1: if node_index != -1:
self.dragging_rotation_handle = True self.dragging_node = True
self.dragging_rotation_handle_index = rotation_handle_index self.dragging_node_index = node_index
else:
rotation_handle_index = self.is_point_in_rotation_handle(x, y)
if rotation_handle_index != -1:
self.dragging_rotation_handle = True
self.dragging_rotation_handle_index = rotation_handle_index
self.last_click_time = current_time self.last_click_time = current_time
self.last_click_pos = pos self.last_click_pos = pos
#Deletes node #Deletes node
def delete_node(self, index): def delete_node(self, index):
self.coordinates = np.delete(self.coordinates, index, axis=0) self.coordinates = np.delete(self.coordinates, index, axis=0)
if index < len(self.handles): if index < len(self.control_points):
del self.handles[index] del self.control_points[index]
if index > 0 and index < len(self.control_points):
del self.control_points[index - 1]
if index < len(self.rotation_handles): if index < len(self.rotation_handles):
del self.rotation_handles[index] del self.rotation_handles[index]
self.draw_scene() self.draw_scene()
#
def mouseMoveEvent(self, event: QMouseEvent): def mouseMoveEvent(self, event: QMouseEvent):
if self.dragging_handle: if self.dragging_control_point:
pos = self.image_label.mapFrom(self, event.position().toPoint()) pos = self.image_label.mapFrom(self, event.position().toPoint())
x, y = pos.x(), pos.y() x, y = pos.x(), pos.y()
self.handles[self.dragging_handle_index] = QPoint(x, y) curve_index, point_index = self.dragging_control_point_index
self.control_points[curve_index][point_index] = QPoint(x, y)
self.draw_scene() self.draw_scene()
elif self.dragging_node: elif self.dragging_node:
pos = self.image_label.mapFrom(self, event.position().toPoint()) pos = self.image_label.mapFrom(self, event.position().toPoint())
x, y = pos.x(), pos.y() x, y = pos.x(), pos.y()
self.coordinates[self.dragging_node_index] = [x, y] self.coordinates[self.dragging_node_index] = [x, y]
self.calculate_handle_pos() self.calculate_control_points()
self.calculate_rotation_handle_pos() self.calculate_rotation_handle_pos()
self.draw_scene() self.draw_scene()
elif self.dragging_rotation_handle: elif self.dragging_rotation_handle:
@@ -144,8 +146,8 @@ class MainWindow(QMainWindow):
#Resets dragging when mouse is released #Resets dragging when mouse is released
def mouseReleaseEvent(self, event: QMouseEvent): def mouseReleaseEvent(self, event: QMouseEvent):
if event.button() == Qt.LeftButton: if event.button() == Qt.LeftButton:
self.dragging_handle = False self.dragging_control_point = False
self.dragging_handle_index = -1 self.dragging_control_point_index = (-1, -1)
self.dragging_node = False self.dragging_node = False
self.dragging_node_index = -1 self.dragging_node_index = -1
self.dragging_rotation_handle = False self.dragging_rotation_handle = False
@@ -159,12 +161,13 @@ class MainWindow(QMainWindow):
return i return i
return -1 return -1
def is_point_in_handle(self, x, y): def is_point_in_control_point(self, x, y):
for i, handle in enumerate(self.handles): for i, control_pair in enumerate(self.control_points):
if (abs(x - handle.x()) <= self.handle_size // 2 and for j, control_point in enumerate(control_pair):
abs(y - handle.y()) <= self.handle_size // 2): if (abs(x - control_point.x()) <= self.handle_size // 2 and
return i abs(y - control_point.y()) <= self.handle_size // 2):
return -1 return (i, j)
return (-1, -1)
def is_point_in_node(self, x, y): def is_point_in_node(self, x, y):
for i, (node_x, node_y) in enumerate(self.coordinates): for i, (node_x, node_y) in enumerate(self.coordinates):
@@ -174,11 +177,15 @@ class MainWindow(QMainWindow):
return -1 return -1
#Calculating the handle positions #Calculating the handle positions
def calculate_handle_pos(self): def calculate_control_points(self):
if len(self.coordinates) >= 2: if len(self.coordinates) >= 2:
x1, y1 = self.coordinates[-2] x1, y1 = self.coordinates[-2]
x2, y2 = self.coordinates[-1] x2, y2 = self.coordinates[-1]
self.handles.append(QPoint((x1 + x2) // 2, (y1 + y2) // 2)) mid_x, mid_y = (x1 + x2) // 2, (y1 + y2) // 2
self.control_points.append([
QPoint(mid_x - (x2 - x1) // 4, mid_y - (y2 - y1) // 4),
QPoint(mid_x + (x2 - x1) // 4, mid_y + (y2 - y1) // 4)
])
def calculate_rotation_handle_pos(self): def calculate_rotation_handle_pos(self):
for i, (x, y) in enumerate(self.coordinates): for i, (x, y) in enumerate(self.coordinates):
@@ -203,40 +210,36 @@ class MainWindow(QMainWindow):
grey_pen = QPen(QColor(127, 127, 127)) grey_pen = QPen(QColor(127, 127, 127))
grey_pen.setWidth(2) grey_pen.setWidth(2)
painter.setPen(grey_pen) painter.setPen(grey_pen)
for i, handle in enumerate(self.handles): for i, control_pair in enumerate(self.control_points):
if i < len(self.coordinates) and i + 1 < len(self.coordinates): if i < len(self.coordinates) - 1:
node1_pos = QPoint(self.coordinates[i][0], self.coordinates[i][1])
node2_pos = QPoint(self.coordinates[i+1][0], self.coordinates[i+1][1])
painter.drawLine(handle, node1_pos)
painter.drawLine(handle, node2_pos)
if len(self.coordinates) >= 2:
for i in range(len(self.coordinates) - 1):
start = QPoint(self.coordinates[i][0], self.coordinates[i][1]) start = QPoint(self.coordinates[i][0], self.coordinates[i][1])
end = QPoint(self.coordinates[i + 1][0], self.coordinates[i + 1][1]) end = QPoint(self.coordinates[i + 1][0], self.coordinates[i + 1][1])
if i < len(self.handles): pen = QPen(Qt.yellow)
pen = QPen(Qt.yellow) pen.setWidth(2)
pen.setWidth(2) painter.setPen(pen)
painter.setPen(pen)
path = QPainterPath() path = QPainterPath()
path.moveTo(start) path.moveTo(start)
handle = self.handles[i] path.cubicTo(control_pair[0], control_pair[1], end)
path.cubicTo(handle, handle, end) painter.drawPath(path)
painter.drawPath(path)
painter.setPen(Qt.NoPen) painter.setBrush(Qt.NoBrush)
painter.setBrush(QColor(0, 255, 255)) for control_point in control_pair:
painter.setPen(QPen(QColor(127, 127, 127), 1, Qt.DashLine))
painter.drawLine(start, control_point)
painter.drawLine(end, control_point)
painter.setPen(Qt.NoPen)
painter.setBrush(QColor(0, 255, 255))
for control_point in control_pair:
painter.drawEllipse( painter.drawEllipse(
handle.x() - self.handle_size // 2, control_point.x() - self.handle_size // 2,
handle.y() - self.handle_size // 2, control_point.y() - self.handle_size // 2,
self.handle_size, self.handle_size,
self.handle_size self.handle_size
) )
painter.setBrush(Qt.NoBrush) painter.setBrush(Qt.NoBrush)
else:
painter.drawLine(start, end)
for i, (x, y) in enumerate(self.coordinates): for i, (x, y) in enumerate(self.coordinates):
if i < len(self.rotation_handles): if i < len(self.rotation_handles):
@@ -259,7 +262,7 @@ class MainWindow(QMainWindow):
font = painter.font() font = painter.font()
font.setPointSize(25) font.setPointSize(25)
painter.setFont(font) painter.setFont(font)
painter.drawText(QRect(x - self.node_size//2, y - self.node_size//2, painter.drawText(QRect(x - self.node_size // 2, y - self.node_size // 2,
self.node_size, self.node_size), self.node_size, self.node_size),
Qt.AlignCenter, str(i + 1)) Qt.AlignCenter, str(i + 1))
@@ -275,34 +278,41 @@ class MainWindow(QMainWindow):
self.image_label.setPixmap(self.pixmap) self.image_label.setPixmap(self.pixmap)
#S M O O T H #S M O O T H
def smoothPoints(self, index: int): def smoothPoints(self, curve_index: int, point_index: int):
curveEditPoints = self.handles if curve_index + 1 < len(self.control_points):
nodes = [QPoint(x, y) for x, y in self.coordinates] prev_control_pair = self.control_points[curve_index]
next_control_pair = self.control_points[curve_index + 1]
node = QPoint(self.coordinates[curve_index + 1][0], self.coordinates[curve_index + 1][1])
for i in range(index+1, len(curveEditPoints)): #Calculate the new position for the second control point
controlPointPos = curveEditPoints[i-1] new_control2 = QPoint(
nodePos = nodes[i] 2 * node.x() - next_control_pair[0].x(),
curveEditPoints[i] = QPoint( 2 * node.y() - next_control_pair[0].y()
2*nodePos.x() - controlPointPos.x(),
2*nodePos.y() - controlPointPos.y()
)
for i in range(0, index):
controlPointPos = curveEditPoints[index-i]
nodePos = nodes[index-i]
curveEditPoints[index-i-1] = QPoint(
2*nodePos.x() - controlPointPos.x(),
2*nodePos.y() - controlPointPos.y()
) )
self.control_points[curve_index][1] = new_control2
self.handles = curveEditPoints #Calculate the new position for the first control point
new_control1 = QPoint(
2 * node.x() - prev_control_pair[1].x(),
2 * node.y() - prev_control_pair[1].y()
)
self.control_points[curve_index + 1][0] = new_control1
#Clears all #Clears all
def clear_points(self): def clear_points(self):
self.coordinates = np.empty((0, 2), dtype=int) self.coordinates = np.empty((0, 2), dtype=int)
self.handles = [] self.control_points.clear()
self.rotation_handles = [] self.rotation_handles.clear()
self.pixmap = QPixmap(os.path.join(os.path.dirname(os.path.abspath(__file__)), "images", "Field.png")) self.node_angles.clear()
self.image_label.setPixmap(self.pixmap) self.draw_scene()
#Warning for clearing
def show_clear_warning(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.clear_points()
if __name__ == "__main__": if __name__ == "__main__":
app = QApplication(sys.argv) app = QApplication(sys.argv)