add ros2 node

node.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float32MultiArray
+from sensor_msgs.msg import JointState
+from utils import RobotClient
+import time
+
+HOST = "192.168.3.2"
+PORT = 9090
+
+config = {
+    "pre_grasp_position": {
+        'pos': [0.14726859839859208, 0.5302551637478977, 0.565792932407347],
+        'ori': [0.1305294928324298, -0.34660161091966263, 0.8293216816453197, 0.4183956538514503]
+    },
+    "grasp_position": {
+        'pos': [0.15928907070579643, 0.5270829242433969, 0.6510703359003733],
+        'ori': [0.15023706706737835, -0.32373703396446113, 0.8482916258752815, 0.39118342754871005]
+    },
+    "post_grasp_position": {
+        'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212],
+        'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941]
+    },
+    "pre_insert_position": {
+        'pos': [0.08296640360052251, 0.5270826520836475, 0.4622718470958212],
+        'ori': [0.15024595915643826, -0.3237308590654784, 0.8482945564918141, 0.3911787674097941]
+    },
+    "insert_position": {
+        "pos": [0.6, 0.0, 0.2],
+        "ori_offset": [10, -5, 0], 
+    },
+    "post_insert_position": {
+        "pos": [0.6, 0.0, 0.2],
+        "ori_offset": [10, -5, 0],
+    },
+}
+
+class RobotControllerNode(Node):
+    def __init__(self):
+        super().__init__('robot_controller_node')
+
+        self.pub = self.create_publisher(Float32MultiArray, "/forward_position_controller/commands", 10)
+        self.js_sub = self.create_subscription(JointState, "/joint_states", self.js_callback, 10)
+        self.last_js = JointState()
+
+        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 = ""
+ 
+        # Запуск основного сценария
+        self.run_sequence()
+
+    def js_callback(self, msg: JointState):
+        self.last_js = msg
+
+    def send_gripper_pose(self, target_pose: float, tolerance: float = 0.01, timeout: float = 3.0):
+        # Публикация команды
+        msg = Float32MultiArray()
+        msg.data = [target_pose]
+        self.pub.publish(msg)
+        self.get_logger().info(f"Sent gripper pose: {target_pose}")
+
+        # Ожидание достижения положения
+        start_time = time.time()
+        while rclpy.ok():
+            if not self.last_js.name:
+                rclpy.spin_once(self, timeout_sec=0.1)
+                continue
+
+            try:
+                index = self.last_js.name.index(self.gripper_joint_name)
+                current_pose = self.last_js.position[index]
+                if abs(current_pose - target_pose) < tolerance:
+                    self.get_logger().info(f"Gripper reached pose: {current_pose:.3f}")
+                    break
+            except ValueError:
+                self.get_logger().warn("Gripper joint name not found in joint_states")
+
+            if time.time() - start_time > timeout:
+                self.get_logger().warn("Timeout waiting for gripper to reach position")
+                break
+
+            rclpy.spin_once(self, timeout_sec=0.1)
+
+    def run_sequence(self):
+        try:
+            self.get_logger().info("Moving to pre-grasp")
+            self.robot_client.move_to_pose_joint(config["pre_grasp_position"])
+            
+            self.get_logger().info("Moving to grasp")
+            self.robot_client.move_to_pose_cart(config["grasp_position"])
+
+            self.get_logger().info("Closing gripper")
+            self.send_gripper_pose(1.0)
+            time.sleep(1.0)
+
+            # self.get_logger().info("Moving to post-grasp")
+            # self.robot_client.move_to_pose_cart(config["post_grasp_position"])
+            #
+            # self.get_logger().info("Moving to pre-insert")
+            # self.robot_client.move_to_pose_cart(config["pre_insert_position"])
+            #
+            # self.get_logger().info("Moving to insert")
+            # self.robot_client.move_to_pose_cart(config["insert_position"])
+            #
+            # self.get_logger().info("Opening gripper")
+            # self.send_gripper_pose(0.0)
+            # time.sleep(1.0)
+            #
+            # self.get_logger().info("Moving to post-insert")
+            # self.robot_client.move_to_pose_cart(config["post_insert_position"])
+            #
+            # self.get_logger().info("Sequence complete")
+
+        finally:
+            self.robot_client.close()
+            self.get_logger().info("Robot client closed")
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = RobotControllerNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()