simplify module3 venue workflow

module3/simple_module_b.py

@@ -14,7 +14,7 @@ from ultralytics import YOLO
 
 
 HERE = Path(__file__).resolve().parent
-CAMERA = "/RMC1/arm95/camera_gripper/image_color"
+CAMERA = "/RMC1/arm95/svcam/right/image"
 GRIPPER = "/RMC1/arm95/gripper_trajectory_controller/joint_trajectory"
 
 
@@ -26,10 +26,10 @@ class Demo(Node):
         self.gripper = self.create_publisher(JointTrajectory, GRIPPER, 10)
         self.create_subscription(Image, CAMERA, self.on_image, qos_profile_sensor_data)
         self.started = None
-        self.opened = False
-        self.closed = False
+        self.last_log = -1
+        self.step = 0
 
-    def move_gripper(self, position):
+    def move_gripper(self, position, state):
         message = JointTrajectory()
         message.joint_names = ["left_joint", "right_joint"]
         point = JointTrajectoryPoint()
@@ -37,6 +37,7 @@ class Demo(Node):
         point.time_from_start = Duration(sec=1)
         message.points = [point]
         self.gripper.publish(message)
+        print(f"[GRIPPER] {state}: {position}", flush=True)
 
     def on_image(self, message):
         frame = self.bridge.imgmsg_to_cv2(message, "bgr8")
@@ -44,16 +45,19 @@ class Demo(Node):
 
         if self.started is None:
             self.started = time.monotonic()
-        elapsed = time.monotonic() - self.started
+            print(f"[CAMERA] {CAMERA}", flush=True)
+        seconds = int(time.monotonic() - self.started)
 
-        if elapsed >= 5 and not self.opened:
-            self.move_gripper(0.04)
-            self.opened = True
-            print("Захват открыт")
-        elif elapsed >= 7 and not self.closed:
-            self.move_gripper(0.0213)
-            self.closed = True
-            print("Захват закрыт")
+        if seconds != self.last_log:
+            print(f"[YOLO] {result.verbose().strip()}", flush=True)
+            self.last_log = seconds
+
+        if seconds >= 5 and self.step == 0:
+            self.move_gripper(0.04, "OPEN")
+            self.step = 1
+        elif seconds >= 7 and self.step == 1:
+            self.move_gripper(0.02, "TAKE_CUBE")
+            self.step = 2
 
         cv2.imshow("YOLO", result.plot())
         if cv2.waitKey(1) & 0xFF in (ord("q"), 27):
@@ -61,7 +65,8 @@ class Demo(Node):
 
 
 def main():
-    input("Предмет (Кисть, Нож или Резаки): ")
+    target = input("Предмет (Кисть, Нож или Резаки): ")
+    print(f"[START] {target}", flush=True)
     rclpy.init()
     node = Demo()
     try: