152 lines
9.5 KiB
Python
152 lines
9.5 KiB
Python
|
import os
|
|||
|
|
import time
|
|||
|
|
import pyrealsense2 as rs
|
|||
|
|
import numpy as np
|
|||
|
|
import cv2
|
|||
|
|
|
|||
|
|
# =============================
|
|||
|
|
# 1. 关节点位定义（20组，每组7个）
|
|||
|
|
# =============================
|
|||
|
|
# JOINT_POSITIONS = [
|
|||
|
|
# [-0.1756037771702, 1.1800090074539, 1.8522456884384, 1.5761110782623, -2.7331359386444, 0.2563314139843, -0.0606026798487],
|
|||
|
|
# [-0.1923076510429, 1.1099290847778, 1.8832376003265, 1.7514026165009, -2.6219944953918, 0.1934144645929, 0.2889180481434],
|
|||
|
|
# [0.2704087197781, 0.9557962417603, 1.7136577367783, 1.6711657047272, -2.5147924423218, 0.0584972538054, 0.783962905407],
|
|||
|
|
# [0.383531242609, 0.7108500599861, 1.7350907325745, 1.8691725730896, -2.2797479629517, -0.1232690215111, 1.2245988845825],
|
|||
|
|
# [0.3922453224659, 0.7737803459167, 1.8068689107895, 1.6336191892624, -2.1118586063385, -0.0338453501463, 1.1426001787186],
|
|||
|
|
# [0.2443187087774, 0.7818641066551, 1.9731577634811, 1.5233805179596, -1.872123837471, 0.0405574627221, 1.1054704189301],
|
|||
|
|
# [0.1276659220457, 0.9156659245491, 2.0081288814545, 1.4481414556503, -1.8519257307053, 0.2532368600368, 0.8394842743874],
|
|||
|
|
# [0.1861536949873, 1.0141937732697, 1.8355123996735, 1.1459348201752, -2.2625010013580, 0.5969776511192, 0.2645072638988],
|
|||
|
|
# [-0.0192848723382, 0.9415585398674, 1.8732609748840, 1.1809715032578, -2.6499755382538, 0.6354467868805, -0.2003904730082],
|
|||
|
|
# [-0.1747295111418, 1.0453623533249, 1.8939573764801, 1.4446462392807, -2.6536157131195, 0.5147834420204, -0.1335265636444],
|
|||
|
|
# [-0.0191576723009, 1.0618592500687, 1.8731262683868, 1.4747705459595, -2.4312825202942, 0.3830471336842, 0.2654716968536],
|
|||
|
|
# [0.1436645090580, 0.8961057662964, 1.9278227090836, 1.5483351945877, -2.1520030498505, 0.2300240248442, 0.7717508077621],
|
|||
|
|
# [-0.2458887547255, 1.3182551860809, 1.8035455942154, 1.7302685976028, -2.8433899879456, 0.0447540767491, -0.0295310281217],
|
|||
|
|
# [-0.1628440171480, 1.1346882581711, 1.7724473476410, 1.8686875104904, -2.8621222972870, 0.0011371960863, 0.3351055085659],
|
|||
|
|
# [0.2227641940117, 0.9846931695938, 1.5547094345093, 1.9238842725754, -2.8571910858154, -0.1052865162492, 0.8961741328239],
|
|||
|
|
# [0.4523145258427, 0.7738097310066, 1.5341449975967, 1.9425786733627, -2.5897336006165, -0.1147941574454, 1.2599183320999],
|
|||
|
|
# [0.6323996782303, 0.7300637960434, 1.3266047239304, 2.0460503101349, -2.7566838264465, -0.1049704179168, 1.4412622451782],
|
|||
|
|
# [0.6435400247574, 0.7136778831482, 1.3097128868103, 2.0483627319336, -2.7511403560638, -0.1215622797608, 1.5219746828079],
|
|||
|
|
# [0.0131992585957, 1.1347451210022, 1.4958723783493, 1.7922105789185, -3.1400320529938, -0.2474720925093, 0.4145260155201],
|
|||
|
|
# [-0.1823624074459, 1.4246475696564, 1.7136197090149, 1.6952623128891, -2.9659118652344, -0.1499162465334, -0.0364842526615],
|
|||
|
|
# ]
|
|||
|
|
|
|||
|
|
# JOINT_POSITIONS = [
|
|||
|
|
# [-0.718823, 1.40293, 2.16063, 1.88231, -2.7615, 0.489431, -0.26], # 0.05 -0.3 -0.2
|
|||
|
|
# [-0.708813, 1.12828, 2.50833, 1.80323, -2.31185, 0.655554, 0.0481596], # 0.05 -0.4 -0.12
|
|||
|
|
# [ -0.741438, 1.25175, 2.32353, 1.87691, -2.52151, 0.551262, -0.127302], # 0.05 -0.35 -0.16
|
|||
|
|
# [-0.450394, 1.09129, 2.19947, 1.44129, -2.64341, 0.614236, -0.26], # 0.1 -0.4 -0.21
|
|||
|
|
# [-0.594676, 1.36527, 1.89605, 1.89043, -2.83642, 0.234229, -0.204878], # 0.1 -0.25 -0.21
|
|||
|
|
# [ -0.741438, 1.25175, 2.32353, 1.87691, -2.52151, 0.551262, -0.127302], # 0.1 -0.33 -0.15
|
|||
|
|
# [-0.390978, 1.16001, 2.05936, 1.90062, -2.5732, 0.280691, 0.125318], # 0.15 -0.31 -0.14
|
|||
|
|
# [-0.373488, 1.2204, 1.93236, 1.6772, -2.73648, 0.297523, -0.140674], # 0.15 -0.3 -0.21
|
|||
|
|
# [-0.401033, 1.34807, 1.77306, 1.81898, -2.87362, 0.135286, -0.111775], # 0.15 -0.23 -0.21
|
|||
|
|
# [ -0.175812, 1.17802, 1.85193, 1.57614, -2.73311, 0.256294, -0.0606398], # 0.2 -0.3 -0.2
|
|||
|
|
# [-0.373488, 1.2204, 1.93236, 1.6772, -2.73648, 0.297523, -0.140674], # 0.2 -0.25 -0.17
|
|||
|
|
# [-0.0984266, 1.07668, 1.88658, 1.2654, -2.72499, 0.414404, -0.234536], # 0.2 -0.36 -0.23
|
|||
|
|
# [0.0871982, 1.09615, 1.7789, 1.19035, -2.75303, 0.346485, -0.173718], # 0.25 -0.34 -0.22
|
|||
|
|
# [ 0.0222405, 1.18277, 1.74292, 1.51783, -2.75674, 0.178517, 0.0381994], # 0.25 -0.28 -0.18
|
|||
|
|
# [0.047511, 1.19839, 1.72007, 1.29276, -2.81499, 0.234448, -0.163403], # 0.25 -0.29 -0.23
|
|||
|
|
# ]
|
|||
|
|
|
|||
|
|
JOINT_POSITIONS = [
|
|||
|
|
[1.1838487386703, 0.4235305488110, 1.2719281911850, 1.5996315479279, -1.8642479181290, -0.1892226040363, 1.5996874570847], # 0.05 -0.3 -0.2
|
|||
|
|
[1.1144028902054, 0.9934917092323, 1.2339128255844, 1.9597420692444, -2.7524161338806, -0.1010338962078, 2.0266277790070], # 0.05 -0.4 -0.12
|
|||
|
|
[0.7585971355438, 1.2986438274384, 1.5511839389801, 1.9102549552917, -3.1083328723907,0.3383120596409, 1.7495325803757], # 0.05 -0.35 -0.16
|
|||
|
|
[0.7717612981796, -0.0227999277413, 1.2341349124908, 0.8974148035049, -1.1484123468399, 1.0799195766449, 1.3140604496002], # 0.1 -0.4 -0.21
|
|||
|
|
[0.3262319564819, 1.4255702495575, 1.0224531888962, 1.6128582954407, -3.1161243915558, -1.0476765632629, 0.5188082456589], # 0.1 -0.25 -0.21
|
|||
|
|
[0.6811909079552, -0.0465415082872, 0.6306891441345, 0.7774662375450, -0.9128333926201, 1.0805308818817, 0.7373644113541], # 0.1 -0.33 -0.15
|
|||
|
|
[0.5894140601158, 0.5544267892838, 0.1245087385178, 0.6875356435776, -1.0255894660950, 1.0803401470184, -0.3253187835217], # 0.15 -0.31 -0.14
|
|||
|
|
[-0.0615737587214, 0.5200612545013, 2.2596797943115, 1.9721800088882, -1.8687928915024, -0.0344338789582, 1.2385385036469], # 0.15 -0.3 -0.21
|
|||
|
|
[0.9264683723450, 1.2653641700745, 1.7594407796860, 1.5681478977203, -2.9171006679535, 0.1166879013181, 1.7105128765106], # 0.15 -0.23 -0.21
|
|||
|
|
[1.0020719766617, 0.4507283568382, 1.0405201911926, 1.0608046054840, -1.5450072288513, 0.1164857149124, 1.1358253955841], # 0.2 -0.3 -0.2
|
|||
|
|
[0.7424476146698, 0.0144095467404, 0.9496489167213, 0.8356217741966, -1.2565077543259, 1.0145307779312, 0.8847945928574], # 0.2 -0.25 -0.17
|
|||
|
|
[0.2278160750866, 0.7444448471069, 0.9402200579643, 0.5896779298782, -1.5535788536072, 1.0801521539688, -0.3112177252769], # 0.2 -0.36 -0.23
|
|||
|
|
[-0.0866613686085, 1.3698191642761, 1.1545568704605, 1.6992814540863, -2.7753651142120, -0.8176611065865, -0.1862865835428], # 0.25 -0.34 -0.22
|
|||
|
|
[0.2748872637749, 1.5703777074814, 1.0753053426743, 1.0457334518433, -2.9064140319824, -0.9773108959198, -0.3075242042542], # 0.25 -0.28 -0.18
|
|||
|
|
[0.2410523593426, 0.9616132378578, 1.6866004467010, 1.9817161560059, -2.7480125427246, 0.0709219276905, 1.1297777891159],
|
|||
|
|
[0.5065582394600, 0.8918085694313, 2.1731114387512, 1.3742331266403, -2.4524819850922, 1.0801626443863, 1.1498324871063],
|
|||
|
|
[0.5651561021805, 0.7603342533112, 1.1436395645142, 1.9550926685333, -1.9175499677658, -1.0519500970840, 0.6535356044769],
|
|||
|
|
[-0.3106776177883, 0.8671860694885, 2.9035902023315, 1.7579817771912, -1.5784481763840, 1.0808564424515, 1.4593633413315],
|
|||
|
|
[0.0202996153384, -0.2583181858063, 1.7621537446976, 1.8765900135040, -1.8188692331314, -0.0442187041044, 0.7810990214348], # 0.25 -0.29 -0.23
|
|||
|
|
]
|
|||
|
|
|
|||
|
|
input_i = int(input("set index: "))
|
|||
|
|
# =============================
|
|||
|
|
# 2. 初始化RealSense
|
|||
|
|
# =============================
|
|||
|
|
TARGET_SERIAL = "243122075614"
|
|||
|
|
ctx = rs.context()
|
|||
|
|
devices = ctx.query_devices()
|
|||
|
|
if len(devices) == 0:
|
|||
|
|
raise RuntimeError("未检测到任何 RealSense 相机，请检查连接")
|
|||
|
|
|
|||
|
|
selected_device = None
|
|||
|
|
for dev in devices:
|
|||
|
|
if dev.get_info(rs.camera_info.serial_number) == TARGET_SERIAL:
|
|||
|
|
selected_device = dev
|
|||
|
|
break
|
|||
|
|
if selected_device is None:
|
|||
|
|
raise RuntimeError(f"未找到序列号为 {TARGET_SERIAL} 的 RealSense 相机")
|
|||
|
|
|
|||
|
|
pipeline = rs.pipeline()
|
|||
|
|
config = rs.config()
|
|||
|
|
config.enable_device(TARGET_SERIAL)
|
|||
|
|
config.enable_stream(rs.stream.depth, 640, 480, rs.format.z16, 30)
|
|||
|
|
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
|
|||
|
|
pipeline.start(config)
|
|||
|
|
|
|||
|
|
align = rs.align(rs.stream.color)
|
|||
|
|
|
|||
|
|
# =============================
|
|||
|
|
# 3. 数据采集流程
|
|||
|
|
# =============================
|
|||
|
|
output_joint_file = "data/20250804-1331/joints_record.txt"
|
|||
|
|
|
|||
|
|
with open(output_joint_file, "a") as f:
|
|||
|
|
# for idx, q in enumerate(JOINT_POSITIONS, start=1):
|
|||
|
|
idx = input_i
|
|||
|
|
q = JOINT_POSITIONS[input_i]
|
|||
|
|
print(f"\n===== 采集点 {idx} =====")
|
|||
|
|
cmd = "moveJ one right " + " ".join(str(v) for v in q)
|
|||
|
|
os.system(cmd)
|
|||
|
|
print("运动完成，等待1秒...")
|
|||
|
|
time.sleep(1)
|
|||
|
|
|
|||
|
|
# 获取关节角度输出
|
|||
|
|
tmp_file = "./data/tmp_joint_output.txt"
|
|||
|
|
os.system(f"getJoint > {tmp_file}")
|
|||
|
|
with open(tmp_file, "r") as tf:
|
|||
|
|
lines = tf.readlines()
|
|||
|
|
joint_output = lines[-1].strip() if lines else ""
|
|||
|
|
print("getJoint 输出：\n", joint_output)
|
|||
|
|
|
|||
|
|
# 保存关节数据
|
|||
|
|
f.write(f"{idx} {joint_output}\n")
|
|||
|
|
f.flush()
|
|||
|
|
|
|||
|
|
# 捕获对齐后的彩色图像
|
|||
|
|
for i in range(10):
|
|||
|
|
try:
|
|||
|
|
frames = pipeline.wait_for_frames(timeout_ms=5000)
|
|||
|
|
if frames:
|
|||
|
|
break
|
|||
|
|
except RuntimeError:
|
|||
|
|
print("⚠️ 读取帧超时，重试中...")
|
|||
|
|
time.sleep(1)
|
|||
|
|
frames = pipeline.wait_for_frames()
|
|||
|
|
aligned_frames = align.process(frames)
|
|||
|
|
color_frame = aligned_frames.get_color_frame()
|
|||
|
|
|
|||
|
|
if not color_frame:
|
|||
|
|
print("未获取彩色图像，跳过")
|
|||
|
|
exit(-1)
|
|||
|
|
|
|||
|
|
color_image = np.asanyarray(color_frame.get_data())
|
|||
|
|
img_path = os.path.join(os.path.dirname(output_joint_file), f"img_{idx}.png")
|
|||
|
|
cv2.imwrite(img_path, color_image)
|
|||
|
|
print(f"保存图像 {img_path}")
|
|||
|
|
|
|||
|
|
# 停止RealSense
|
|||
|
|
pipeline.stop()
|
|||
|
|
print("\n=== 数据采集完成 ===")
|