openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
shenchenyang 2026-03-18 14:33:58 +08:00
parent 8cda2b4c2a
commit 0e267ce936
4 changed files with 115 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/openarm_camera/launch/launch_camera.launch.py
View File

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

101
src/openarm_camera/openarm_camera/camera_pub.py
View File

@ -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)
# 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

15
src/openarm_camera/openarm_camera/stereo_hand_depth_node.py
View File

@ -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)) # 注意这里是左图宽度

27
src/openarm_servo_control/openarm_servo_control/ik_controller.py
View File

@ -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])