diff --git a/gui/palletizing.py b/gui/palletizing.py index 9a3e81c..307b93f 100644 --- a/gui/palletizing.py +++ b/gui/palletizing.py @@ -5,9 +5,10 @@ from logger import logger class Palletizing(QWidget): - def __init__(self, load_palletizing): + def __init__(self, load_palletizing, start_moving): super().__init__() self.load_palletizing = load_palletizing + self.start_moving = start_moving self.initUI() def initUI(self): @@ -19,12 +20,18 @@ class Palletizing(QWidget): self.palletButton = QPushButton("Визуализировать") self.palletButton.clicked.connect(self.startPalletzing) self.layout.addWidget(self.palletButton) + + self.startButton = QPushButton("Конвейер") + self.startButton.clicked.connect(self.startMoving) + self.layout.addWidget(self.startButton) self.setLayout(self.layout) def startPalletzing(self): - logger.info("start palletizing") self.load_palletizing() + + def startMoving(self): + self.start_moving() def paintEvent(self, event): p = self.palette() diff --git a/main.py b/main.py index bd85ce2..c8c8da8 100644 --- a/main.py +++ b/main.py @@ -73,6 +73,7 @@ class MyApp: }, palletizingPanel={ "load_palletizing": self.load_palletizing, + "start_moving": self.start_moving }, ) mainWindow.setWindowTitle("ROBOT GUI") @@ -181,6 +182,10 @@ class MyApp: @check_robot_app def load_palletizing(self): return self.robot_app.load_palletizing() + + @check_robot_app + def start_moving(self): + return self.robot_app.start_moving() if __name__ == "__main__": diff --git a/robot/client_socket.py b/robot/client_socket.py index 666c6cf..ba31583 100644 --- a/robot/client_socket.py +++ b/robot/client_socket.py @@ -77,6 +77,9 @@ class SocketRobotArm: def load_palletizing(self): self.urdf_manager.load_palletizing() + + def start_moving(self): + self.urdf_manager.conveyor.moving = True def get_pybullet_image(self): return self.urdf_manager.get_pybullet_image() diff --git a/robot/conveyor.py b/robot/conveyor.py index 33b2955..c8ab3f6 100644 --- a/robot/conveyor.py +++ b/robot/conveyor.py @@ -5,9 +5,20 @@ from logger import logger class ConveyorBelt: + moving = False + def __init__( - self, size, position, box_size, box_spacing, box_speed, palletizinFunc + self, + size, + position, + direction, + box_size, + box_spacing, + box_speed, + palletizinFunc, ): + self.direction = direction + self.conveyor_size = size self.conveyor_position = position self.conveyor_id = self.create_conveyor(size, position) @@ -44,21 +55,19 @@ class ConveyorBelt: return conveyor_id def box_position(self, i=0): - x_position = ( - self.conveyor_position[0] - - self.conveyor_size[0] * 0.5 - + i * (self.box_size[0] + self.box_spacing) + position = self.conveyor_position[:] + position[self.direction] = ( + self.conveyor_position[self.direction] + - self.conveyor_size[self.direction] * 0.5 + + i * (self.box_size[self.direction] + self.box_spacing) + + self.box_size[0] * 0.5 ) - y_position = ( - self.conveyor_position[1] + self.conveyor_size[1] * 0.5 - self.box_size[1] - ) - z_position = ( - self.conveyor_position[2] + self.conveyor_size[2] + self.box_size[2] * 0.5 - ) - return [x_position, y_position, z_position] + return position def create_boxes(self): - num_boxes = int(self.conveyor_size[0] / (self.box_size[0] + self.box_spacing)) + num_boxes = int( + self.conveyor_size[self.direction] / (self.box_size[0] + self.box_spacing) + ) for i in range(num_boxes): position = self.box_position(i) self.boxes.append(self.create_box(position)) @@ -91,23 +100,25 @@ class ConveyorBelt: return box_id def move_boxes(self): - if not self.conveyor_stopped: + if self.moving and not self.conveyor_stopped: for box_id in self.boxes[:]: pos, orn = p.getBasePositionAndOrientation(box_id) - if ( - pos[0] + self.box_size[0] * 0.5 - > self.conveyor_position[0] + self.conveyor_size[0] * 0.5 + pos[self.direction] + self.box_size[0] * 0.5 + > self.conveyor_position[self.direction] + + self.conveyor_size[self.direction] * 0.5 ): # Если коробка выходит за конец конвейера, она падает вниз (физика продолжает действовать) # Удаляем коробку с конвейера self.boxes.remove(box_id) self.conveyor_stopped = True - right_wall_position = [pos[0], pos[1] + self.box_size[1] * 0.5, pos[2]] - self.call_robot(right_wall_position, box_id) + wall_position = list(pos) + dir_wall = (self.direction + 1) % 3 + wall_position[dir_wall] += self.box_size[dir_wall] + self.call_robot(wall_position, box_id) else: - # Двигаем коробку вдоль конвейера (по оси X) - new_pos = [pos[0] + self.box_speed, pos[1], pos[2]] + new_pos = list(pos[:]) + new_pos[self.direction] += self.box_speed p.resetBasePositionAndOrientation(box_id, new_pos, orn) # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество diff --git a/robot/urdf_manager.py b/robot/urdf_manager.py index de4e8b7..692bff5 100644 --- a/robot/urdf_manager.py +++ b/robot/urdf_manager.py @@ -54,15 +54,22 @@ class UrdfManager: ) self.pallet_id = p.loadURDF(urdf_path_pallet, pallet_position) - self.conv_pos = [-1.5, -1.5, 0.5] - self.conv_size = [3.0, 0.5, 0.1] - ConveyorBelt( + # положение дальнего угла + self.conv_target = [1.35, -0.75, 0.5] + self.conv_size = [0.5, 3.0, 0.1] + self.conv_direction = self.conv_size.index(max(self.conv_size)) + self.conv_pos = [ + (self.conv_target[i] - self.conv_size[i] * 0.5) + for i in range(len(self.conv_target)) + ] + self.conveyor = ConveyorBelt( self.conv_size, self.conv_pos, + self.conv_direction, [0.2, 0.2, 0.2], 0.2, 0.1, - self.palletizing + self.palletizing, ) time.sleep(1)