支持键盘控制

src/openarm_servo_control/openarm_servo_control/ik_controller.py

@@ -89,9 +89,10 @@ class ArucoIKController(Node):
             max_acceleration=0.01,   # 建议 0.2~0.6，降低以减小加速度
             dt=self.control_period
         )
-        self.Kp = 20.0                   # 比例增益（需调试）
+        self.Kp = 15.0                   # 比例增益（需调试）
-        self.max_vel = 3.0              # rad/s 每关节上限
+        self.max_vel = 2.5              # rad/s 每关节上限
-        self.max_acc = 1.0              # rad/s^2 加速度限幅
+        self.max_acc = 0.1              # rad/s^2 加速度限幅
+        self.currPose = PoseStamped()
 
         self.pose_queue = queue.Queue(maxsize=10)  # 用于存储最近的目标位姿，避免处理过旧的指令
         self.ik_ready = False
@@ -136,15 +137,15 @@ class ArucoIKController(Node):
             "/compute_ik"
         )
 
-        self.float64MultiArrayPublisher_ = self.create_publisher(
+        # self.float64MultiArrayPublisher_ = self.create_publisher(
-            Float64MultiArray, 
+        #     Float64MultiArray, 
-            "/left_forward_position_controller/commands", 
+        #     "/left_forward_position_controller/commands", 
-            10)
+        #     10)
         
         self.create_subscription(
             PoseStamped,
             "/hand_3d",   # 已经转到 base 的 ArUco
-            self.producer,
+            self.dataProcess,
             10
         )
 
@@ -159,9 +160,9 @@ class ArucoIKController(Node):
         # )
 
         self.timer = self.create_timer(3.0, self.check_ik_service)
-        # time.sleep(3)
+        time.sleep(3)
         self.timer1 = self.create_timer(0.05, self.on_pose)
-        self.test_timer = self.create_timer(0.05, self.test_callback)
+        self.startTimer = self.create_timer(0.05, self.readyArm)
         # 用于计算速度/加速度（用于平滑输出）
         self.prev_q = None
         self.prev_vel = None
@@ -169,6 +170,31 @@ class ArucoIKController(Node):
         # 如果为 False，则同时发布 positions/vel/acc 给 position/trajectory 控制器。
         self.publish_velocity_only = False
     
+    def dataProcess(self, pose: PoseStamped):
+        posebuf = self.currPose
+        posebuf.pose.position.x += pose.pose.position.x
+        posebuf.pose.position.y += pose.pose.position.y
+        posebuf.pose.position.z += pose.pose.position.z
+        posebuf.pose.orientation.x = 1.0
+        posebuf.pose.orientation.y = 0.0
+        posebuf.pose.orientation.z = 0.0
+        posebuf.pose.orientation.w = 0.0
+        self.producer(posebuf)
+    
+    def readyArm(self):
+        if self.count < 0.2:
+            self.count += 0.005
+            self.currPose.pose.position.x = 0.0
+            self.currPose.pose.position.y = 0.0
+            self.currPose.pose.position.z = self.count
+            self.currPose.pose.orientation.x = 1.0
+            self.currPose.pose.orientation.y = 0.0
+            self.currPose.pose.orientation.z = 0.0
+            self.currPose.pose.orientation.w = 0.0
+            self.producer(self.currPose)
+        else:
+            self.startTimer.cancel()
+
     def test_callback(self):
         if self.count > 0.2:
             self.countFlag = True

src/openarm_servo_control/openarm_servo_control/keyboard_ctrl.py

@@ -0,0 +1,92 @@
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseStamped
+
+import threading
+import sys
+import termios
+import tty
+
+def get_key():
+    fd = sys.stdin.fileno()
+    old_settings = termios.tcgetattr(fd)
+
+    try:
+        tty.setraw(fd)
+        ch = sys.stdin.read(1)
+
+        if ch == '\x1b':
+            ch += sys.stdin.read(2)
+            if ch == '\x1b[':
+                ch += sys.stdin.read(1)
+
+        return ch
+
+    finally:
+        termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
+
+class KeyboardCtrlNode(Node):
+    def __init__(self):
+        super().__init__('keyboard_ctrl_node')
+        self.get_logger().info('正在初始化Keyboard控制节点...')
+        # 在这里添加键盘控制的初始化代码，例如订阅键盘输入的话题或设置定时器等
+
+        self.pub = self.create_publisher(
+            PoseStamped,
+            "/hand_3d",
+            10
+        )
+
+        self.running = True
+
+        # 启动键盘线程
+        self.thread = threading.Thread(target=self.keyboard_loop)
+        self.thread.start()
+
+    def keyboard_loop(self):
+
+        while self.running:
+
+            key = get_key()
+
+            msg = PoseStamped()
+            msg.header.frame_id = "base_link"
+
+            if key == '\x1b[A':  # up
+                msg.pose.position.x = 0.01
+            elif key == '\x1b[B':  # down
+                msg.pose.position.x = -0.01
+
+            elif key == '\x1b[C':  # right
+                msg.pose.position.y = -0.01
+
+            elif key == '\x1b[D':  # left
+                msg.pose.position.y = 0.01
+
+            elif key == '.':  # period
+                msg.pose.position.z = -0.01
+
+            elif key == ';':  # semicolon
+                msg.pose.position.z = 0.01
+
+            elif key == 'q':
+                self.running = False
+                self.destroy_node()
+                rclpy.shutdown()
+                break
+            else:
+                continue
+
+            self.pub.publish(msg)
+
+            self.get_logger().info("Command sent")
+
+def main():
+    rclpy.init()
+    node = KeyboardCtrlNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()

src/openarm_servo_control/setup.py

@@ -26,6 +26,7 @@ setup(
         'console_scripts': [
             'servo_control_node = openarm_servo_control.servo_control:main',
             'ik_controller_node = openarm_servo_control.ik_controller:main',
+            'keyboard_ctrl_node = openarm_servo_control.keyboard_ctrl:main',
         ],
     },
 )