From 82c92827ea1aae3f229ee04ec77e2e0db4ee444b Mon Sep 17 00:00:00 2001
From: Bill Finger <ur.narmak@gmail.com>
Date: Fri, 23 May 2025 20:27:35 +0300
Subject: [PATCH] use thread instead async

---
 node.py | 61 ++++++++++++++++++++++-----------------------------------
 1 file changed, 23 insertions(+), 38 deletions(-)

diff --git a/node.py b/node.py
index f60528a..e668028 100644
--- a/node.py
+++ b/node.py
@@ -7,7 +7,7 @@ from sensor_msgs.msg import JointState
 from utils import RobotClient
 import time
 from main import config, HOST, PORT
-import asyncio
+import threading
 
 
 class RobotControllerNode(Node):
@@ -24,26 +24,21 @@ class RobotControllerNode(Node):
 
         self.robot_client = RobotClient(HOST, PORT)
         self.get_logger().info("Robot client connected")
-        self.loop = asyncio.get_event_loop()
         self.waiting_for_gripper = False
 
-        # Настройка скорости и ускорения
         self.robot_client.set_end_max_line_velc(0.6)
         self.robot_client.set_end_max_line_acc(0.8)
         self.gripper_joint_name = "left_outer_knuckle_joint"
         self.sequence_step = 0
 
-        # Запуск основного сценария через таймер
-        self.timer = self.create_timer(
-            1.0, self.run_sequence
-        )  # Задержка 1 сек для инициализации
+        self.timer = self.create_timer(1.0, self.run_sequence)
 
     def js_callback(self, msg: JointState):
         self.last_js = msg
 
-    async def send_gripper_command_async(
+    def send_gripper_command(
         self, target_pose: float, tolerance: float = 0.01, timeout: float = 3.0
-    ):
+    ) -> bool:
         msg = Float64MultiArray()
         msg.data = [target_pose]
         self.pub.publish(msg)
@@ -68,39 +63,27 @@ class RobotControllerNode(Node):
                         )
                         return True
                 else:
-                    self.get_logger().warn(
-                        "Gripper joint index out of bounds in position list"
-                    )
+                    self.get_logger().warn("Gripper joint index out of bounds")
             else:
-                self.get_logger().warn(
-                    f"Gripper joint '{self.gripper_joint_name}' not found in joint_states"
-                )
+                self.get_logger().warn(f"Joint '{self.gripper_joint_name}' not found")
                 return False
 
             if time.time() - start_time > timeout:
-                self.get_logger().warn("Timeout waiting for gripper to reach position")
+                self.get_logger().warn("Timeout waiting for gripper")
                 return False
 
-    async def handle_gripper_step(
+    def handle_gripper_step(
         self, target_pose: float, timeout: float = 3.0, tolerance: float = 0.01
     ):
         try:
-            success = await self.send_gripper_command_async(
-                target_pose, tolerance, timeout
-            )
+            success = self.send_gripper_command(target_pose, tolerance, timeout)
             if success:
-                self.sequence_step += 1
-                self.get_logger().info(
-                    f"Gripper action succeeded, moving to step {self.sequence_step}"
-                )
+                self.get_logger().info("Gripper moved successfully")
+                self.waiting_for_gripper = False
             else:
-                self.get_logger().warn(
-                    f"Gripper action failed or timed out at pose {target_pose}"
-                )
+                self.get_logger().warn("Gripper movement failed or timed out.")
         except Exception as e:
-            self.get_logger().error(f"Exception in handle_gripper_step: {str(e)}")
-        finally:
-            self.waiting_for_gripper = False
+            self.get_logger().error(f"Exception in gripper thread: {str(e)}")
 
     def run_sequence(self):
         print(f"STEP #{self.sequence_step}")
@@ -117,11 +100,12 @@ class RobotControllerNode(Node):
         elif self.sequence_step == 2:
             if not self.waiting_for_gripper:
                 self.waiting_for_gripper = True
-                self.loop.create_task(
-                    self.handle_gripper_step(
-                        target_pose=0.582, timeout=10.0, tolerance=0.1
-                    )
-                )
+                threading.Thread(
+                    target=self.handle_gripper_step,
+                    args=(0.582, 10.0, 0.1),
+                    daemon=True,
+                ).start()
+                self.sequence_step += 1
 
         elif self.sequence_step == 3:
             self.get_logger().info("Moving to post-grasp")
@@ -146,9 +130,10 @@ class RobotControllerNode(Node):
         elif self.sequence_step == 7:
             if not self.waiting_for_gripper:
                 self.waiting_for_gripper = True
-                self.loop.create_task(
-                    self.handle_gripper_step(target_pose=0.54, timeout=10.0)
-                )
+                threading.Thread(
+                    target=self.handle_gripper_step, args=(0.54, 10.0), daemon=True
+                ).start()
+                self.sequence_step += 1
 
         elif self.sequence_step >= 8:
             self.get_logger().info("Sequence complete")