优化摄像头帧率，增加手部姿态控制

src/openarm_camera/launch/launch_camera.launch.py

@@ -44,7 +44,7 @@ def generate_launch_description():
         # actions=[camera_cal_init]
         # ),
         TimerAction(
-        period=8.0,
+        period=4.0,
         actions=[stereo_hand_depth]
         ),
     ])

src/openarm_camera/openarm_camera/camera_pub.py

@@ -4,15 +4,18 @@ from sensor_msgs.msg import Image
 from cv_bridge import CvBridge
 import cv2
 import numpy as np
+import time
 
 from camera_info_manager import CameraInfoManager
 from sensor_msgs.msg import CameraInfo
 
 CAMERA_WIDTH = 1280
 CAMERA_HEIGHT = 480
+TARGET_FPS = 30
 
 FRAME_WIDTH = int(CAMERA_WIDTH / 2)
 FRAME_HEIGHT = int(CAMERA_HEIGHT / 2)
+DIAG_INTERVAL_SEC = 1.0
 
 print(FRAME_WIDTH, FRAME_HEIGHT)
  
@@ -33,7 +36,7 @@ class CameraNode(Node):
         # self.left_camera_info_manager.setURL(left_camera_info_url)
         # self.right_camera_info_manager.setURL(right_camera_info_url)
 
-        print("2ds1as23d123sa1d32as", self.left_camera_info_manager.getURL())
+        
         self.left_camera_info_manager.loadCameraInfo()
         self.right_camera_info_manager.loadCameraInfo()
         # 然后，你可以通过CameraInfoManager获取CameraInfo消息
@@ -48,6 +51,15 @@ class CameraNode(Node):
         self.publisher_left = self.create_publisher(Image, '/camera/left/image_raw', 10)
         self.publisher_right = self.create_publisher(Image, '/camera/right/image_raw', 10)
         self.publisher_raw = self.create_publisher(Image, '/image_1', 10)
+
+        self.diag_last_time = time.perf_counter()
+        self.capture_ok_count = 0
+        self.publish_count = 0
+        self.read_cost_sum = 0.0
+        self.convert_cost_sum = 0.0
+        self.publish_cost_sum = 0.0
+        self.total_cost_sum = 0.0
+
         self.get_logger().info('正在初始化摄像头...')
     
         
@@ -55,12 +67,15 @@ class CameraNode(Node):
         self.cap = None
         for i in range(2):  # 尝试设备0,1,2
             self.get_logger().info(f'尝试打开摄像头设备 /dev/video{i}...')
-            self.cap = cv2.VideoCapture(i)
+            self.cap = cv2.VideoCapture(i, cv2.CAP_V4L2)
             if self.cap.isOpened():
+                # 先申请 MJPG，再设置分辨率/FPS；该设备在 1280x480 下 YUYV 仅支持 5FPS。
+                self.cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc(*'MJPG'))
                 # 设置摄像头参数
                 self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, CAMERA_WIDTH)
                 self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, CAMERA_HEIGHT)
-                self.cap.set(cv2.CAP_PROP_FPS, 30)
+                self.cap.set(cv2.CAP_PROP_FPS, TARGET_FPS)
+                self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
                 
                 # 让摄像头预热
                 for _ in range(5):
@@ -71,35 +86,85 @@ class CameraNode(Node):
                 if ret and frame is not None:
                     self.get_logger().info(f'成功打开物理摄像头 /dev/video{i}')
                     self.get_logger().info(f'图像尺寸: {frame.shape}')
+                    real_w = int(self.cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+                    real_h = int(self.cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+                    real_fps = self.cap.get(cv2.CAP_PROP_FPS)
+                    fourcc = int(self.cap.get(cv2.CAP_PROP_FOURCC))
+                    fourcc_str = ''.join([chr((fourcc >> 8 * j) & 0xFF) for j in range(4)])
+                    self.get_logger().info(
+                        f'驱动实际参数: {real_w}x{real_h}, fps={real_fps:.2f}, fourcc={fourcc_str} (target_fps={TARGET_FPS})'
+                    )
+                    if fourcc_str != 'MJPG':
+                        self.get_logger().warn(
+                            '当前不是MJPG模式,1280x480下可能会被限制到低帧率。'
+                        )
                     break
 
+        if self.cap is None or not self.cap.isOpened():
+            raise RuntimeError('未找到可用摄像头设备，请检查 /dev/video* 和权限')
 
-        self.timer = self.create_timer(0.001, self.timer_callback)  # 30Hz
+        self.timer = self.create_timer(1.0 / TARGET_FPS, self.timer_callback)
         self.get_logger().info('摄像头节点已启动')
     
     def timer_callback(self):
+        t0 = time.perf_counter()
         ret, frame = self.cap.read()
+        t1 = time.perf_counter()
+
         if ret and frame is not None:
-            # frameDist = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE)
+            stamp = self.get_clock().now().to_msg()
-            # cv2.imshow("camera_left", frame[:, :800])
+            # msg_left= self.bridge.cv2_to_imgmsg(frame[:, :FRAME_WIDTH], encoding='bgr8')
-            # cv2.imshow("camera_right", frame[:, 800:])
+            # msg_right= self.bridge.cv2_to_imgmsg(frame[:, FRAME_WIDTH:], encoding='bgr8')
-            cv2.waitKey(0)
-            time = self.get_clock().now().to_msg()
-            msg_left= self.bridge.cv2_to_imgmsg(frame[:, :FRAME_WIDTH], encoding='bgr8')
-            msg_right= self.bridge.cv2_to_imgmsg(frame[:, FRAME_WIDTH:], encoding='bgr8')
             msg_raw= self.bridge.cv2_to_imgmsg(frame, encoding='bgr8')
-            msg_left.header.stamp = time
+            t2 = time.perf_counter()
-            msg_right.header.stamp = time
+
-            msg_raw.header.stamp = time
+            # msg_left.header.stamp = stamp
-            msg_left.header.frame_id = 'camera_frame_left'
+            # msg_right.header.stamp = stamp
-            msg_right.header.frame_id = 'camera_frame_right'
+            msg_raw.header.stamp = stamp
+            # msg_left.header.frame_id = 'camera_frame_left'
+            # msg_right.header.frame_id = 'camera_frame_right'
             msg_raw.header.frame_id = 'camera_frame_raw'
-            self.publisher_left.publish(msg_left)
+            # self.publisher_left.publish(msg_left)
-            self.publisher_right.publish(msg_right)
+            # self.publisher_right.publish(msg_right)
             self.publisher_raw.publish(msg_raw)
+            t3 = time.perf_counter()
+
             # 发布相机信息
             self.left_camera_info_pub.publish(self.left_camera_info)
             self.right_camera_info_pub.publish(self.right_camera_info)
+            t4 = time.perf_counter()
+
+            self.capture_ok_count += 1
+            self.publish_count += 1
+            self.read_cost_sum += (t1 - t0)
+            self.convert_cost_sum += (t2 - t1)
+            self.publish_cost_sum += (t3 - t2)
+            self.total_cost_sum += (t4 - t0)
+
+            now = t4
+            elapsed = now - self.diag_last_time
+            if elapsed >= DIAG_INTERVAL_SEC:
+                cap_fps = self.capture_ok_count / elapsed
+                pub_fps = self.publish_count / elapsed
+                avg_read_ms = (self.read_cost_sum / max(self.capture_ok_count, 1)) * 1000.0
+                avg_convert_ms = (self.convert_cost_sum / max(self.publish_count, 1)) * 1000.0
+                avg_publish_ms = (self.publish_cost_sum / max(self.publish_count, 1)) * 1000.0
+                avg_total_ms = (self.total_cost_sum / max(self.publish_count, 1)) * 1000.0
+
+                self.get_logger().info(
+                    f'[diag] cap_fps={cap_fps:.2f}, pub_fps={pub_fps:.2f}, '
+                    f'read={avg_read_ms:.2f}ms, convert={avg_convert_ms:.2f}ms, '
+                    f'publish={avg_publish_ms:.2f}ms, total={avg_total_ms:.2f}ms'
+                )
+
+                self.diag_last_time = now
+                self.capture_ok_count = 0
+                self.publish_count = 0
+                self.read_cost_sum = 0.0
+                self.convert_cost_sum = 0.0
+                self.publish_cost_sum = 0.0
+                self.total_cost_sum = 0.0
+            
         
 
 

src/openarm_camera/openarm_camera/stereo_hand_depth_node.py

@@ -139,8 +139,18 @@ class StereoHandDepthNode(Node):
 
         # 左右拆分
         left = frame[:, :mid]
-        right = frame[:, mid:]
+        # right = frame[:, mid:]
         left_rect = cv2.remap(left, self.map1_l, self.map2_l, cv2.INTER_LINEAR)
+        left_h, left_w = left_rect.shape[:2]
+        # 在 left_rect 图像中心画一个 10x10 像素的绿色框（BGR (0,255,0)）
+        cx = left_w // 2
+        cy = left_h // 2
+        half = 5  # 半边长度，10x10 框
+        tl = (max(cx - half, 0), max(cy - half, 0))
+        br = (min(cx + half, left_w - 1), min(cy + half, left_h - 1))
+        cv2.rectangle(left_rect, tl, br, (0, 255, 0), 1)
+        
+        # self.get_logger().info(f"Left rectified size: {left_w}x{left_h}")
         # right_rect = cv2.remap(right, self.map1_r, self.map2_r, cv2.INTER_LINEAR)
         # results = self.mp_hands.process(left_rect)
         # p = PointStamped()
@@ -205,7 +215,7 @@ class StereoHandDepthNode(Node):
                     
                     qx, qy, qz, qw = rot.as_quat()
                     roll, pitch, yaw = rot.as_euler('xyz')
-                    self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})")
+                    # self.get_logger().info(f"quat: ({qx}, {qy}, {qz}, {qw})")
                     self.get_logger().info(f"euler: ({roll*180/3.14:.1f}, {pitch*180/3.14:.1f}, {yaw*180/3.14:.1f})")
 
                     # 记录时间
@@ -213,6 +223,7 @@ class StereoHandDepthNode(Node):
 
                     # 记录最后一次有效目标
                     self.last_valid_pos = np.array([t_ros[0] + 0.7, t_ros[1], t_ros[2]])
+                    self.get_logger().info(f"Marker position (ROS frame): ({self.last_valid_pos[0]:.3f}, {self.last_valid_pos[1]:.3f}, {self.last_valid_pos[2]:.3f})")
                     self.last_valid_quat = np.array([qx, qy, qz, qw])
                     
             # u = int(u * (w / 2))  # 注意这里是左图宽度

src/openarm_servo_control/openarm_servo_control/ik_controller.py

@@ -172,13 +172,20 @@ class ArucoIKController(Node):
     
     def dataProcess(self, pose: PoseStamped):
         posebuf = self.currPose
-        posebuf.pose.position.x += pose.pose.position.x
+        # posebuf.pose.position.x += pose.pose.position.x
-        posebuf.pose.position.y += pose.pose.position.y
+        # posebuf.pose.position.y += pose.pose.position.y
-        posebuf.pose.position.z += pose.pose.position.z
+        # posebuf.pose.position.z += pose.pose.position.z
-        posebuf.pose.orientation.x = 1.0
+        posebuf.pose.position.x = pose.pose.position.x
-        posebuf.pose.orientation.y = 0.0
+        posebuf.pose.position.y = pose.pose.position.y
-        posebuf.pose.orientation.z = 0.0
+        posebuf.pose.position.z = pose.pose.position.z + 0.2
-        posebuf.pose.orientation.w = 0.0
+        # posebuf.pose.orientation.x = 1.0
+        # posebuf.pose.orientation.y = 0.0
+        # posebuf.pose.orientation.z = 0.0
+        # posebuf.pose.orientation.w = 0.0
+        posebuf.pose.orientation.x = pose.pose.orientation.x
+        posebuf.pose.orientation.y = pose.pose.orientation.y
+        posebuf.pose.orientation.z = pose.pose.orientation.z
+        posebuf.pose.orientation.w = pose.pose.orientation.w
         self.producer(posebuf)
     
     def readyArm(self):
@@ -262,7 +269,7 @@ class ArucoIKController(Node):
         #     self.count += 0.004
 
         if self.pose_queue.empty():
-            self.get_logger().warn("Pose queue empty, waiting for pose...")
+            # self.get_logger().warn("Pose queue empty, waiting for pose...")
             return
 
         pose = self.pose_queue.get()
@@ -280,7 +287,7 @@ class ArucoIKController(Node):
             return
         curr_pos = np.array([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z])
         curr_quat = np.array([pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w])
-        # self.get_logger().info(f"Received target pose: {curr_pos}, {curr_quat}")
+        self.get_logger().info(f"Received target pose: {curr_pos}, {curr_quat}")
             
         req = GetPositionIK.Request()
         req.ik_request.group_name = self.arm_group
@@ -304,7 +311,7 @@ class ArucoIKController(Node):
         # pose.pose.orientation.w = 0.0
 
         # curr_pos = np.array([0.0, 0.0, 0.0])  # 相对于机械臂 base 的位置
-        curr_quat = np.array([1.0, 0.0, 0.0, 0.0])
+        # curr_quat = np.array([1.0, 0.0, 0.0, 0.0])