conv pos

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()

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__":

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()

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)
 
         # Добавляем новые коробки, если на конвейере меньше, чем максимально возможное количество

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)