From aa04898d87141eac93ba137465abb2a19cce450e Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Fri, 23 May 2025 20:07:28 +0300 Subject: [PATCH] async gripper command sender --- node.py | 93 +++++++++++++++++++++++++++++++-------------------------- 1 file changed, 50 insertions(+), 43 deletions(-) diff --git a/node.py b/node.py index cbfa356..17bf951 100644 --- a/node.py +++ b/node.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 import rclpy +import asyncio from rclpy.node import Node from std_msgs.msg import Float64MultiArray from sensor_msgs.msg import JointState @@ -24,33 +25,31 @@ class RobotControllerNode(Node): self.robot_client = RobotClient(HOST, PORT) self.get_logger().info("Robot client connected") - # Настройка скорости и ускорения 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" # Укажите правильное имя joint'а + self.gripper_joint_name = "left_outer_knuckle_joint" self.sequence_step = 0 + self.waiting_for_gripper = False - # Запуск основного сценария через таймер - 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 - def send_gripper_command(self, target_pose: float, tolerance: float = 0.01, timeout: float = 3.0): + async def send_gripper_command( + self, target_pose: float, tolerance: float = 0.01, timeout: float = 3.0 + ): msg = Float64MultiArray() msg.data = [target_pose] self.pub.publish(msg) self.get_logger().info(f"Sent gripper pose: {target_pose}") - # Ждём данных от joint_states start_time = time.time() - while rclpy.ok(): - rclpy.spin_once(self, timeout_sec=0.1) - + while time.time() - start_time < timeout: joint_name_list = list(self.last_js.name) if self.last_js.name else [] - joint_position_list = list(self.last_js.position) if self.last_js.position else [] + joint_position_list = ( + list(self.last_js.position) if self.last_js.position else [] + ) if self.gripper_joint_name in joint_name_list: index = joint_name_list.index(self.gripper_joint_name) @@ -58,20 +57,25 @@ class RobotControllerNode(Node): if index < len(joint_position_list): current_pose = joint_position_list[index] if abs(current_pose - target_pose) < tolerance: - self.get_logger().info(f"Gripper reached pose: {current_pose:.3f}") + self.get_logger().info( + f"Gripper reached pose: {current_pose:.3f}" + ) 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") - return False + self.get_logger().warn( + f"Joint {self.gripper_joint_name} not in joint_states" + ) - if time.time() - start_time > timeout: - self.get_logger().warn("Timeout waiting for gripper to reach position") - return False + await asyncio.sleep(0.1) + + self.get_logger().warn("Timeout waiting for gripper") + return False def run_sequence(self): print(f"STEP #{self.sequence_step}") + if self.sequence_step == 0: self.get_logger().info("Moving to pre-grasp") self.robot_client.move_to_pose_joint(config["pre_grasp_position"]) @@ -83,9 +87,9 @@ class RobotControllerNode(Node): self.sequence_step += 1 elif self.sequence_step == 2: - if self.send_gripper_command(target_pose=0.582, timeout=10.0, tolerance=0.1): - self.get_logger().info("Gripper closed") - self.sequence_step += 1 + if not self.waiting_for_gripper: + self.waiting_for_gripper = True + asyncio.create_task(self.handle_gripper_step(0.582, 0.1, 10.0)) elif self.sequence_step == 3: self.get_logger().info("Moving to post-grasp") @@ -93,50 +97,53 @@ class RobotControllerNode(Node): self.sequence_step += 1 elif self.sequence_step == 4: - self.get_logger().info("Moving to pre_insert_position") + self.get_logger().info("Moving to pre-insert") self.robot_client.move_to_pose_cart(config["pre_insert_position"]) self.sequence_step += 1 elif self.sequence_step == 5: - self.get_logger().info("Moving to insert_position") + self.get_logger().info("Moving to insert") self.robot_client.move_to_pose_joint(config["insert_position"]) self.sequence_step += 1 elif self.sequence_step == 6: - self.get_logger().info("Moving to post_insert_position") + self.get_logger().info("Moving to post-insert") self.robot_client.move_to_pose_joint(config["post_insert_position"]) self.sequence_step += 1 elif self.sequence_step == 7: - if self.send_gripper_command(target_pose=0.54, timeout=10.0): - self.get_logger().info("Gripper closed") - self.sequence_step += 1 + if not self.waiting_for_gripper: + self.waiting_for_gripper = True + asyncio.create_task(self.handle_gripper_step(0.54, 0.01, 10.0)) - # - # ... и так далее для остальных шагов - - elif self.sequence_step >= 8: # Измените на последний шаг + 1 + elif self.sequence_step >= 8: self.get_logger().info("Sequence complete") self.robot_client.close() self.destroy_timer(self.timer) + async def handle_gripper_step(self, pose, tolerance, timeout): + if await self.send_gripper_command(pose, tolerance, timeout): + self.get_logger().info("Gripper action successful") + self.sequence_step += 1 + else: + self.get_logger().warn("Gripper action failed or timed out") + + self.waiting_for_gripper = False + def main(args=None): rclpy.init(args=args) + node = RobotControllerNode() + # Обязательно многопоточный исполнитель! + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + + loop = asyncio.get_event_loop() try: - node = RobotControllerNode() - executor = rclpy.executors.MultiThreadedExecutor() - executor.add_node(node) - - try: - executor.spin() - finally: - node.destroy_node() - - except Exception as e: - print(f"Fatal error: {str(e)}") + loop.run_until_complete(loop.run_in_executor(None, executor.spin)) finally: + node.destroy_node() rclpy.shutdown()