diff --git a/src/openarm_camera/launch/launch_camera.launch.py b/src/openarm_camera/launch/launch_camera.launch.py index 29777f5..ba491ba 100644 --- a/src/openarm_camera/launch/launch_camera.launch.py +++ b/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] ), ]) diff --git a/src/openarm_camera/openarm_camera/camera_pub.py b/src/openarm_camera/openarm_camera/camera_pub.py index c5b43ca..349a4b1 100644 --- a/src/openarm_camera/openarm_camera/camera_pub.py +++ b/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 - - self.timer = self.create_timer(0.001, self.timer_callback) # 30Hz + if self.cap is None or not self.cap.isOpened(): + raise RuntimeError('未找到可用摄像头设备，请检查 /dev/video* 和权限') + + 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) - # cv2.imshow("camera_left", frame[:, :800]) - # cv2.imshow("camera_right", frame[:, 800:]) - 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') + stamp = 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 - msg_right.header.stamp = time - msg_raw.header.stamp = time - msg_left.header.frame_id = 'camera_frame_left' - msg_right.header.frame_id = 'camera_frame_right' + t2 = time.perf_counter() + + # msg_left.header.stamp = stamp + # msg_right.header.stamp = stamp + 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_right.publish(msg_right) + # self.publisher_left.publish(msg_left) + # 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 + diff --git a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py b/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py index ef18e37..e5afd17 100644 --- a/src/openarm_camera/openarm_camera/stereo_hand_depth_node.py +++ b/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)) # 注意这里是左图宽度 diff --git a/src/openarm_servo_control/openarm_servo_control/ik_controller.py b/src/openarm_servo_control/openarm_servo_control/ik_controller.py index de25d65..2d5b0cd 100644 --- a/src/openarm_servo_control/openarm_servo_control/ik_controller.py +++ b/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.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 + # 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.position.x = pose.pose.position.x + posebuf.pose.position.y = pose.pose.position.y + posebuf.pose.position.z = pose.pose.position.z + 0.2 + # 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])