movefasta/aubo-old-sdk-test
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

use thread instead async

Browse source
This commit is contained in:
Ilya Uraev 2025-05-23 20:27:35 +03:00
parent 77f4e1a831
commit 82c92827ea
1 changed files with 23 additions and 38 deletions
Download patch file Download diff file Expand all files Collapse all files

61
node.py
View file

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