simplify module3 gripper demo

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: