simplify module3 gripper demo

module3/SIMPLE.md

@@ -50,9 +50,9 @@ cd ~/scripts/mvch
 module3/.venv/bin/python module3/simple_module_b.py
 ```
 
-Ввод предмета нужен только для показа. Скрипт выводит YOLO в окне OpenCV, через
-5 секунд открывает захват, а через 2 секунды переводит его в положение
-`take_cube`. Выход — `Q` или `Esc`.
+Сначала открывается живое окно YOLO. Затем в терминале появляется пустой выбор
+предмета. После Enter захват закрывается, а через 2 секунды открывается. Выход —
+`Q` или `Esc`.
 
 Камера реального RMC1: `/RMC1/arm95/svcam/right/image`. Для проверки в Webots
 замените константу `CAMERA` на `/RMC1/arm95/camera_gripper/image_color`.

module3/simple_module_b.py

@@ -1,6 +1,7 @@
 #!/usr/bin/env python3
 import time
 from pathlib import Path
+from threading import Thread
 
 import cv2
 import rclpy
@@ -25,8 +26,7 @@ class Demo(Node):
         self.bridge = CvBridge()
         self.gripper = self.create_publisher(JointTrajectory, GRIPPER, 10)
         self.create_subscription(Image, CAMERA, self.on_image, qos_profile_sensor_data)
-        self.started = None
-        self.step = 0
+        self.asked = False
 
     def move_gripper(self, position):
         message = JointTrajectory()
@@ -37,28 +37,25 @@ class Demo(Node):
         message.points = [point]
         self.gripper.publish(message)
 
+    def ask_target(self):
+        input("Выберите предмет (Кисть, Нож или Резаки): ")
+        self.move_gripper(0.0)
+        time.sleep(2)
+        self.move_gripper(0.04)
+
     def on_image(self, message):
         frame = self.bridge.imgmsg_to_cv2(message, "bgr8")
         result = self.model(frame, conf=0.25, verbose=False)[0]
 
-        if self.started is None:
-            self.started = time.monotonic()
-        seconds = int(time.monotonic() - self.started)
-
-        if seconds >= 5 and self.step == 0:
-            self.move_gripper(0.04)
-            self.step = 1
-        elif seconds >= 7 and self.step == 1:
-            self.move_gripper(0.02)
-            self.step = 2
-
         cv2.imshow("YOLO", result.plot())
+        if not self.asked:
+            self.asked = True
+            Thread(target=self.ask_target, daemon=True).start()
         if cv2.waitKey(1) & 0xFF in (ord("q"), 27):
             rclpy.shutdown()
 
 
 def main():
-    input("Предмет (Кисть, Нож или Резаки): ")
     rclpy.init()
     node = Demo()
     try: