aaaaaa/car
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Initial commit

Browse source
This commit is contained in:
1 2026-08-13 14:35:17 +08:00
commit a4e596c3aa
234 changed files with 81101 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

BIN
arm_plane_calib/biaodi.png Normal file
View file

Binary file not shown.
Side by side

After

Width:  |  Height:  |  Size: 143 KiB

185
arm_plane_calib/collect_plane_calib.py Normal file
View file

@ -0,0 +1,185 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import yaml
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
class PlaneCalibCollector(Node):
def __init__(self):
super().__init__('plane_calib_collector')
self.declare_parameter('color_topic', '/camera/color/image_raw')
self.declare_parameter('depth_topic', '/camera/depth/image_raw')
self.declare_parameter('camera_info_topic', '/camera/depth/camera_info')
self.declare_parameter('save_path', 'samples.yaml')
self.declare_parameter('depth_scale', 0.001) # 深度单位转米，常见 mm -> m
self.declare_parameter('min_depth_m', 0.05)
self.declare_parameter('max_depth_m', 1.50)
self.declare_parameter('median_kernel', 5)
self.declare_parameter('target_count', 9) # 采满多少个点自动退出
self.color_topic = self.get_parameter('color_topic').value
self.depth_topic = self.get_parameter('depth_topic').value
self.camera_info_topic = self.get_parameter('camera_info_topic').value
self.save_path = self.get_parameter('save_path').value
self.depth_scale = float(self.get_parameter('depth_scale').value)
self.min_depth_m = float(self.get_parameter('min_depth_m').value)
self.max_depth_m = float(self.get_parameter('max_depth_m').value)
self.median_kernel = int(self.get_parameter('median_kernel').value)
self.target_count = int(self.get_parameter('target_count').value)
self.bridge = CvBridge()
self.color_img = None
self.depth_img = None
self.K = None
self.samples = []
self.should_exit = False
self.create_subscription(Image, self.color_topic, self.on_color, 10)
self.create_subscription(Image, self.depth_topic, self.on_depth, 10)
self.create_subscription(CameraInfo, self.camera_info_topic, self.on_info, 10)
self.window_name = 'plane_calib_click'
cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL)
cv2.setMouseCallback(self.window_name, self.on_mouse)
self.get_logger().info('左键点击标定点，终端输入机械臂命令向量，例如: 500,620,410,530')
self.get_logger().info(f'每记录 1 个样本自动保存，采满 {self.target_count} 个样本自动退出')
self.get_logger().info('按 q 可手动退出')
def on_color(self, msg: Image):
self.color_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
def on_depth(self, msg: Image):
if msg.encoding in ('16UC1', 'mono16'):
self.depth_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
else:
self.get_logger().warn(f'不支持的深度编码: {msg.encoding}')
def on_info(self, msg: CameraInfo):
self.K = np.array(msg.k, dtype=np.float64).reshape(3, 3)
def get_xyz_from_pixel(self, u: int, v: int):
if self.depth_img is None or self.K is None:
return None
h, w = self.depth_img.shape[:2]
if not (0 <= u < w and 0 <= v < h):
return None
k = self.median_kernel
r = k // 2
u0, u1 = max(0, u - r), min(w, u + r + 1)
v0, v1 = max(0, v - r), min(h, v + r + 1)
patch = self.depth_img[v0:v1, u0:u1].astype(np.float32)
valid = patch[patch > 0]
if valid.size == 0:
return None
z = np.median(valid) * self.depth_scale
if z < self.min_depth_m or z > self.max_depth_m:
return None
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
x = (u - cx) * z / fx
y = (v - cy) * z / fy
return float(x), float(y), float(z)
def save_samples(self):
data = {
'samples': self.samples,
'cmd_dim': len(self.samples[0]['arm_cmd']) if self.samples else 0
}
with open(self.save_path, 'w', encoding='utf-8') as f:
yaml.safe_dump(data, f, allow_unicode=True, sort_keys=False)
print(f'[SAVE] 已保存到 {self.save_path}')
def on_mouse(self, event, x, y, flags, param):
if event != cv2.EVENT_LBUTTONDOWN:
return
xyz = self.get_xyz_from_pixel(x, y)
if xyz is None:
print(f'[WARN] 点击点 ({x}, {y}) 无有效深度')
return
X, Y, Z = xyz
print(f'\n点击像素: ({x}, {y})')
print(f'相机坐标: X={X:.4f} m, Y={Y:.4f} m, Z={Z:.4f} m')
print('请手动把机械臂末端移到这个点正上方，然后输入命令向量，例如 500,620,410,530')
cmd_str = input('arm_cmd> ').strip()
if not cmd_str:
print('[SKIP] 未输入，跳过')
return
try:
arm_cmd = [float(s) for s in cmd_str.split(',')]
except Exception:
print('[ERR] 输入格式错误，应类似 500,620,410,530')
return
item = {
'u': int(x),
'v': int(y),
'x_cam': X,
'y_cam': Y,
'z_cam': Z,
'arm_cmd': arm_cmd
}
self.samples.append(item)
print(f'[OK] 已记录第 {len(self.samples)} 个样本')
# 自动保存
self.save_samples()
# 采满自动退出
if len(self.samples) >= self.target_count:
print(f'[DONE] 已采满 {self.target_count} 个样本，自动保存并退出')
self.should_exit = True
def spin_loop(self):
while rclpy.ok() and not self.should_exit:
rclpy.spin_once(self, timeout_sec=0.03)
if self.color_img is not None:
show = self.color_img.copy()
for i, s in enumerate(self.samples):
cv2.circle(show, (s['u'], s['v']), 4, (0, 255, 0), -1)
cv2.putText(
show, str(i + 1), (s['u'] + 6, s['v'] - 6),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 1
)
cv2.imshow(self.window_name, show)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
print('[EXIT] 用户退出')
break
cv2.destroyAllWindows()
def main():
rclpy.init()
node = PlaneCalibCollector()
try:
node.spin_loop()
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

344
arm_plane_calib/depth_click_to_learm_cmd_node.py Normal file
View file

@ -0,0 +1,344 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time
import yaml
import cv2
import numpy as np
import serial
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
class LeArmSerial:
def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
self.port = port
self.baudrate = baudrate
self.ser = None
def open(self):
self.ser = serial.Serial()
self.ser.port = self.port
self.ser.baudrate = self.baudrate
self.ser.bytesize = serial.EIGHTBITS
self.ser.parity = serial.PARITY_NONE
self.ser.stopbits = serial.STOPBITS_ONE
self.ser.timeout = 0.1
self.ser.xonxoff = False
self.ser.rtscts = False
self.ser.dsrdtr = False
self.ser.dtr = False
self.ser.rts = False
self.ser.open()
print(f"[INFO] 串口已打开: {self.port} @ {self.baudrate}")
print("[INFO] 等待控制板稳定...")
time.sleep(3.0)
self.ser.reset_input_buffer()
self.ser.reset_output_buffer()
def close(self):
if self.ser and self.ser.is_open:
self.ser.close()
print("[INFO] 串口已关闭")
@staticmethod
def build_frame(cmd: int, params: bytes = b"") -> bytes:
# 协议: 55 55 Length CMD Params...
# Length = CMD(1) + Params长度
length = 1 + len(params)
return bytes([0x55, 0x55, length, cmd]) + params
def move_servos(self, servo_pairs, move_time_ms=800):
"""
servo_pairs: [(servo_id, pulse), ...]
CMD=3 多舵机控制
"""
if not servo_pairs:
print("[WARN] 空舵机列表")
return
params = bytearray()
params.append(len(servo_pairs))
params.append(move_time_ms & 0xFF)
params.append((move_time_ms >> 8) & 0xFF)
for sid, pulse in servo_pairs:
sid = int(sid)
pulse = int(pulse)
params.append(sid & 0xFF)
params.append(pulse & 0xFF)
params.append((pulse >> 8) & 0xFF)
frame = self.build_frame(0x03, bytes(params))
print("TX:", frame.hex(" "))
self.ser.write(frame)
self.ser.flush()
class DepthClickToLeArm(Node):
def __init__(self):
super().__init__("depth_click_to_learm_cmd")
# 相机话题
self.declare_parameter("depth_topic", "/camera/depth/image_raw")
self.declare_parameter("camera_info_topic", "/camera/depth/camera_info")
self.declare_parameter("color_topic", "/camera/color/image_raw")
# 模型
self.declare_parameter("model_path", "model.yaml")
# 深度参数
self.declare_parameter("depth_scale", 0.001) # 16UC1 常见 mm->m
self.declare_parameter("min_depth_m", 0.05)
self.declare_parameter("max_depth_m", 1.20)
self.declare_parameter("median_kernel", 5)
# 串口
self.declare_parameter("serial_port", "/dev/ttyUSB0")
self.declare_parameter("baud_rate", 9600)
self.declare_parameter("move_time_ms", 800)
# 视图
self.declare_parameter("show_color_view", True)
self.declare_parameter("draw_text", True)
# 命令向量 -> 舵机ID映射
# 这里的顺序必须和 model.yaml 输出的命令向量顺序一致
# 默认给一组常见顺序，你后面按自己机械臂实际改
self.declare_parameter("servo_ids", [6, 5, 4, 3])
# 每个舵机范围，按你给出的范围设置
self.declare_parameter("servo_min", [125, 125, 125, 125])
self.declare_parameter("servo_max", [875, 875, 875, 875])
# 如果想把夹爪加入抓取动作，可在点击后自己再加固定动作
self.declare_parameter("auto_send", True)
self.depth_topic = self.get_parameter("depth_topic").value
self.camera_info_topic = self.get_parameter("camera_info_topic").value
self.color_topic = self.get_parameter("color_topic").value
self.model_path = self.get_parameter("model_path").value
self.depth_scale = float(self.get_parameter("depth_scale").value)
self.min_depth_m = float(self.get_parameter("min_depth_m").value)
self.max_depth_m = float(self.get_parameter("max_depth_m").value)
self.median_kernel = int(self.get_parameter("median_kernel").value)
self.serial_port = self.get_parameter("serial_port").value
self.baud_rate = int(self.get_parameter("baud_rate").value)
self.move_time_ms = int(self.get_parameter("move_time_ms").value)
self.show_color_view = bool(self.get_parameter("show_color_view").value)
self.draw_text = bool(self.get_parameter("draw_text").value)
self.servo_ids = list(self.get_parameter("servo_ids").value)
self.servo_min = list(self.get_parameter("servo_min").value)
self.servo_max = list(self.get_parameter("servo_max").value)
self.auto_send = bool(self.get_parameter("auto_send").value)
# 读取模型
with open(self.model_path, "r", encoding="utf-8") as f:
model = yaml.safe_load(f)
self.W = np.array(model["weights"], dtype=np.float64) # [6, cmd_dim]
self.cmd_dim = int(model["cmd_dim"])
if self.cmd_dim != len(self.servo_ids):
raise RuntimeError(
f"model cmd_dim={self.cmd_dim} 与 servo_ids 数量={len(self.servo_ids)} 不一致"
)
self.bridge = CvBridge()
self.depth_img = None
self.color_img = None
self.K = None
self.last_click = None
self.window_name = "depth_click_to_learm_cmd"
self.arm = LeArmSerial(self.serial_port, self.baud_rate)
self.arm.open()
self.create_subscription(Image, self.depth_topic, self.on_depth, 10)
self.create_subscription(CameraInfo, self.camera_info_topic, self.on_info, 10)
self.create_subscription(Image, self.color_topic, self.on_color, 10)
cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL)
cv2.setMouseCallback(self.window_name, self.on_mouse)
self.get_logger().info("节点已启动，左键点击目标点即可预测并发送 CMD=3。")
def destroy_node(self):
try:
self.arm.close()
except Exception:
pass
super().destroy_node()
def on_depth(self, msg: Image):
self.depth_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="passthrough")
def on_color(self, msg: Image):
try:
self.color_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
except Exception:
self.color_img = None
def on_info(self, msg: CameraInfo):
self.K = np.array(msg.k, dtype=np.float64).reshape(3, 3)
@staticmethod
def features(x, y):
return np.array([1.0, x, y, x * x, x * y, y * y], dtype=np.float64)
def pixel_to_xyz(self, u, v):
if self.depth_img is None or self.K is None:
return None
h, w = self.depth_img.shape[:2]
if not (0 <= u < w and 0 <= v < h):
return None
r = self.median_kernel // 2
u0, u1 = max(0, u - r), min(w, u + r + 1)
v0, v1 = max(0, v - r), min(h, v + r + 1)
patch = self.depth_img[v0:v1, u0:u1].astype(np.float32)
valid = patch[patch > 0]
if valid.size == 0:
return None
z = float(np.median(valid) * self.depth_scale)
if z < self.min_depth_m or z > self.max_depth_m:
return None
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
x = float((u - cx) * z / fx)
y = float((v - cy) * z / fy)
return x, y, z
def predict_cmd(self, x_cam, y_cam):
feat = self.features(x_cam, y_cam) # [6]
cmd = feat @ self.W # [cmd_dim]
return cmd
def clamp_cmd(self, cmd):
out = []
for i, v in enumerate(cmd):
lo = float(self.servo_min[i])
hi = float(self.servo_max[i])
out.append(int(round(max(lo, min(hi, v)))))
return out
def send_cmd_vector(self, cmd_vec):
servo_pairs = list(zip(self.servo_ids, cmd_vec))
self.arm.move_servos(servo_pairs, move_time_ms=self.move_time_ms)
def on_mouse(self, event, x, y, flags, param):
if event != cv2.EVENT_LBUTTONDOWN:
return
xyz = self.pixel_to_xyz(x, y)
if xyz is None:
print(f"[WARN] 点击点 ({x}, {y}) 无有效深度")
return
x_cam, y_cam, z_cam = xyz
pred = self.predict_cmd(x_cam, y_cam)
pred_clamped = self.clamp_cmd(pred)
self.last_click = {
"u": int(x),
"v": int(y),
"x_cam": x_cam,
"y_cam": y_cam,
"z_cam": z_cam,
"pred_raw": pred.tolist(),
"pred_cmd": pred_clamped,
}
print("\n========== 点击目标点 ==========")
print(f"像素: ({x}, {y})")
print(f"相机坐标: X={x_cam:.4f} m, Y={y_cam:.4f} m, Z={z_cam:.4f} m")
print("预测原始命令向量:", ", ".join([f"{v:.2f}" for v in pred.tolist()]))
print("限幅后命令向量:", pred_clamped)
print("舵机映射:", list(zip(self.servo_ids, pred_clamped)))
if self.auto_send:
self.send_cmd_vector(pred_clamped)
print("[OK] 已直接发送 CMD=3")
def make_depth_vis(self):
if self.depth_img is None:
return None
depth = self.depth_img.astype(np.float32) * self.depth_scale
depth = np.clip(depth, self.min_depth_m, self.max_depth_m)
norm = (depth - self.min_depth_m) / max(1e-6, (self.max_depth_m - self.min_depth_m))
norm = (norm * 255.0).astype(np.uint8)
vis = cv2.applyColorMap(255 - norm, cv2.COLORMAP_JET)
return vis
def spin_loop(self):
while rclpy.ok():
rclpy.spin_once(self, timeout_sec=0.03)
show = None
if self.show_color_view and self.color_img is not None:
show = self.color_img.copy()
else:
vis = self.make_depth_vis()
if vis is not None:
show = vis
if show is not None and self.last_click is not None:
u = self.last_click["u"]
v = self.last_click["v"]
cv2.circle(show, (u, v), 6, (0, 255, 0), -1)
if self.draw_text:
txt1 = f"({u},{v})"
txt2 = f"X={self.last_click['x_cam']:.3f} Y={self.last_click['y_cam']:.3f} Z={self.last_click['z_cam']:.3f}"
txt3 = "CMD=" + ",".join([str(v) for v in self.last_click["pred_cmd"]])
cv2.putText(show, txt1, (u + 8, v - 24), cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 255, 0), 1)
cv2.putText(show, txt2, (u + 8, v - 8), cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0, 255, 0), 1)
cv2.putText(show, txt3, (u + 8, v + 10), cv2.FONT_HERSHEY_SIMPLEX, 0.50, (0, 255, 0), 1)
cv2.imshow(self.window_name, show)
elif show is not None:
cv2.imshow(self.window_name, show)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
break
elif key == ord("s") and self.last_click is not None:
# 手动再发送一次
self.send_cmd_vector(self.last_click["pred_cmd"])
print("[OK] 手动再次发送 CMD=3")
elif key == ord("c"):
self.last_click = None
cv2.destroyAllWindows()
def main():
rclpy.init()
node = DepthClickToLeArm()
try:
node.spin_loop()
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

60
arm_plane_calib/fit_plane_calib.py Normal file
View file

@ -0,0 +1,60 @@
#!/usr/bin/env python3
import yaml
import numpy as np
import sys
def features(x, y):
return np.array([1.0, x, y, x * x, x * y, y * y], dtype=np.float64)
def main():
sample_path = sys.argv[1] if len(sys.argv) > 1 else 'samples.yaml'
model_path = sys.argv[2] if len(sys.argv) > 2 else 'model.yaml'
with open(sample_path, 'r', encoding='utf-8') as f:
data = yaml.safe_load(f)
samples = data['samples']
if len(samples) < 6:
print('样本太少，至少需要 6 个，建议 9~16 个')
return
X = []
Y = []
for s in samples:
X.append(features(s['x_cam'], s['y_cam']))
Y.append(s['arm_cmd'])
X = np.array(X, dtype=np.float64) # [N, 6]
Y = np.array(Y, dtype=np.float64) # [N, cmd_dim]
# 最小二乘
W, _, _, _ = np.linalg.lstsq(X, Y, rcond=None) # [6, cmd_dim]
Y_pred = X @ W
err = Y_pred - Y
mae = np.mean(np.abs(err), axis=0)
rmse = np.sqrt(np.mean(err ** 2, axis=0))
model = {
'feature_order': ['1', 'x', 'y', 'x2', 'xy', 'y2'],
'weights': W.tolist(),
'cmd_dim': int(Y.shape[1]),
'sample_count': int(len(samples)),
'mae_per_dim': mae.tolist(),
'rmse_per_dim': rmse.tolist(),
'z_ref_median': float(np.median([s['z_cam'] for s in samples]))
}
with open(model_path, 'w', encoding='utf-8') as f:
yaml.safe_dump(model, f, allow_unicode=True, sort_keys=False)
print(f'模型已保存到: {model_path}')
print('每维 MAE:', mae)
print('每维 RMSE:', rmse)
if __name__ == '__main__':
main()

242
arm_plane_calib/learm_test.py Normal file
View file

@ -0,0 +1,242 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
import time
import serial
class LeArmSerial:
def __init__(self, port="/dev/ttyUSB0", baudrate=9600):
self.port = port
self.baudrate = baudrate
self.ser = None
def open(self):
self.ser = serial.Serial()
self.ser.port = self.port
self.ser.baudrate = self.baudrate
self.ser.bytesize = serial.EIGHTBITS
self.ser.parity = serial.PARITY_NONE
self.ser.stopbits = serial.STOPBITS_ONE
self.ser.timeout = 0.1
self.ser.xonxoff = False
self.ser.rtscts = False
self.ser.dsrdtr = False
self.ser.dtr = False
self.ser.rts = False
self.ser.open()
print(f"[INFO] 串口已打开: {self.port} @ {self.baudrate}")
print("[INFO] 等待控制板稳定...")
time.sleep(3.0)
self.ser.reset_input_buffer()
self.ser.reset_output_buffer()
def close(self):
if self.ser and self.ser.is_open:
self.ser.close()
print("[INFO] 串口已关闭")
@staticmethod
def build_frame(cmd: int, params: bytes = b"") -> bytes:
length = 2 + len(params)
return bytes([0x55, 0x55, length, cmd]) + params
def send_frame(self, cmd: int, params: bytes = b""):
frame = self.build_frame(cmd, params)
print("TX:", frame.hex(" "))
self.ser.write(frame)
self.ser.flush()
def read_frames(self, wait_s: float = 1.0):
deadline = time.time() + wait_s
buf = bytearray()
while time.time() < deadline:
n = self.ser.in_waiting
if n > 0:
buf.extend(self.ser.read(n))
time.sleep(0.01)
print("RAW RX:", bytes(buf).hex(" "))
return self.split_frames(bytes(buf))
@staticmethod
def split_frames(buf: bytes):
frames = []
i = 0
while i <= len(buf) - 3:
if buf[i] == 0x55 and buf[i + 1] == 0x55:
length = buf[i + 2]
frame_len = 2 + length
if i + frame_len <= len(buf):
frames.append(buf[i:i + frame_len])
i += frame_len
continue
i += 1
return frames
@staticmethod
def parse_fw_query(frame: bytes):
if len(frame) != 6 or frame[:4] != b"\x55\x55\x04\x01":
return None
servo_type = frame[4]
fw_ver = frame[5]
return {
"servo_type": servo_type,
"servo_type_text": {1: "PWM舵机", 2: "总线舵机"}.get(servo_type, f"未知({servo_type})"),
"firmware_version": fw_ver,
}
@staticmethod
def parse_servo_readback(frame: bytes):
if len(frame) < 6 or frame[0] != 0x55 or frame[1] != 0x55 or frame[3] != 0x0D:
return None
payload = frame[4:]
items = []
if len(payload) % 3 != 0:
usable = len(payload) - (len(payload) % 3)
payload = payload[:usable]
for i in range(0, len(payload), 3):
sid = payload[i]
pulse = payload[i + 1] | (payload[i + 2] << 8)
items.append({"id": sid, "pulse": pulse})
return items
def query_firmware(self):
self.ser.reset_input_buffer()
self.send_frame(0x01)
frames = self.read_frames(wait_s=1.0)
ok = False
for f in frames:
print("FRAME:", f.hex(" "))
info = self.parse_fw_query(f)
if info:
ok = True
print("[OK] 固件查询成功")
print("舵机类型:", info["servo_type_text"])
print("固件版本号:", info["firmware_version"])
if not ok:
print("[WARN] 未解析到有效固件查询返回")
def read_servo_angles(self):
self.ser.reset_input_buffer()
self.send_frame(0x0D)
frames = self.read_frames(wait_s=1.0)
ok = False
for f in frames:
print("FRAME:", f.hex(" "))
items = self.parse_servo_readback(f)
if items is not None:
ok = True
print("[OK] 舵机角度回读成功")
for item in items:
print(f" 舵机ID={item['id']}, 脉宽={item['pulse']}")
if not ok:
print("[WARN] 未解析到有效舵机角度回读返回")
def set_init_pose(self):
self.ser.reset_input_buffer()
self.send_frame(0x0C)
time.sleep(1.5)
print("[OK] 已发送初始位姿设置命令")
def move_servos(self, servo_pairs, move_time_ms=800):
"""
servo_pairs: [(servo_id, pulse), ...]
协议:
长度 = 舵机个数*3 + 5
params:
个数(1B), time_low(1B), time_high(1B),
[id, pulse_low, pulse_high] * N
"""
if not servo_pairs:
print("[WARN] 空舵机列表")
return
params = bytearray()
params.append(len(servo_pairs))
params.append(move_time_ms & 0xFF)
params.append((move_time_ms >> 8) & 0xFF)
for sid, pulse in servo_pairs:
pulse = int(pulse)
params.append(int(sid) & 0xFF)
params.append(pulse & 0xFF)
params.append((pulse >> 8) & 0xFF)
self.ser.reset_input_buffer()
self.send_frame(0x03, bytes(params))
time.sleep(max(move_time_ms / 1000.0, 0.5))
print("[OK] 已发送多舵机控制命令")
def interactive(self):
menu = """
================ LeArm 串口测试 =================
1. 固件查询
2. 舵机角度回读
3. 初始位姿设置
4. 控制单个舵机
5. 控制多个舵机
q. 退出
舵机范围
(1夹爪,200-700;2腕部,125-875;)
(3号,125-875;4号,125-875;)
(5号,125-875;6旋转,125-875;)
=================================================
"""
while True:
print(menu)
choice = input("请选择: ").strip().lower()
if choice == "1":
self.query_firmware()
elif choice == "2":
self.read_servo_angles()
elif choice == "3":
self.set_init_pose()
elif choice == "4":
sid = int(input("舵机ID: ").strip())
pulse = int(input("目标位置(例如 500): ").strip())
t = int(input("运行时间ms(例如 800): ").strip() or "800")
self.move_servos([(sid, pulse)], move_time_ms=t)
elif choice == "5":
print("输入格式示例: 1:500,2:600,3:450")
raw = input("舵机列表: ").strip()
t = int(input("运行时间ms(例如 800): ").strip() or "800")
pairs = []
for item in raw.split(","):
sid_s, pulse_s = item.split(":")
pairs.append((int(sid_s), int(pulse_s)))
self.move_servos(pairs, move_time_ms=t)
elif choice == "q":
break
else:
print("[WARN] 无效选项")
def main():
arm = LeArmSerial(port="/dev/ttyUSB0", baudrate=9600)
try:
arm.open()
arm.interactive()
finally:
arm.close()
if __name__ == "__main__":
main()

45
arm_plane_calib/model.yaml Normal file
View file

@ -0,0 +1,45 @@
feature_order:
- '1'
- x
- y
- x2
- xy
- y2
weights:
- - 511.28515454089643
- 821.2615901131778
- 265.21874233385535
- 398.5549181928104
- - -771.8329814607213
- 52.76710377195036
- 237.64443893556836
- -114.8132656337159
- - 362.52663422740244
- -580.4727764870705
- -26.751203474622418
- -1877.4142059960259
- - 680.0875101829623
- 7153.497267194028
- 5925.331927860114
- 1758.558407685675
- - -4993.565252238856
- 4809.674284044076
- 17834.265848385203
- -12695.665144123212
- - 5570.783049089005
- 3393.015647299803
- 30196.06155860567
- -33269.65533447665
cmd_dim: 4
sample_count: 9
mae_per_dim:
- 17.308270436659697
- 9.353204865258602
- 23.274954237277775
- 26.493102071291403
rmse_per_dim:
- 20.835859382625955
- 12.3444244473184
- 28.89964968573881
- 35.21007477027654
z_ref_median: 0.34400000000000003

152
arm_plane_calib/predict_plane_cmd.py Normal file
View file

@ -0,0 +1,152 @@
#!/usr/bin/env python3
import yaml
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from cv_bridge import CvBridge
def features(x, y):
return np.array([1.0, x, y, x * x, x * y, y * y], dtype=np.float64)
class PlaneCmdPredictor(Node):
def __init__(self):
super().__init__('plane_cmd_predictor')
self.declare_parameter('color_topic', '/camera/color/image_raw')
self.declare_parameter('depth_topic', '/camera/depth/image_raw')
self.declare_parameter('camera_info_topic', '/camera/depth/camera_info')
self.declare_parameter('model_path', 'model.yaml')
self.declare_parameter('depth_scale', 0.001)
self.color_topic = self.get_parameter('color_topic').value
self.depth_topic = self.get_parameter('depth_topic').value
self.camera_info_topic = self.get_parameter('camera_info_topic').value
self.model_path = self.get_parameter('model_path').value
self.depth_scale = float(self.get_parameter('depth_scale').value)
with open(self.model_path, 'r', encoding='utf-8') as f:
model = yaml.safe_load(f)
self.W = np.array(model['weights'], dtype=np.float64) # [6, cmd_dim]
self.bridge = CvBridge()
self.color_img = None
self.depth_img = None
self.K = None
# 你的模型输出顺序固定为：6号、5号、4号、3号
self.model_servo_order = [6, 5, 4, 3]
# 你想打印成：3号、4号、5号、6号
self.print_servo_order = [3, 4, 5, 6]
# 当前只处理 3/4/5/6 号舵机，范围都一致
self.servo_min = 125
self.servo_max = 875
self.create_subscription(Image, self.color_topic, self.on_color, 10)
self.create_subscription(Image, self.depth_topic, self.on_depth, 10)
self.create_subscription(CameraInfo, self.camera_info_topic, self.on_info, 10)
self.window_name = 'predict_plane_cmd'
cv2.namedWindow(self.window_name, cv2.WINDOW_NORMAL)
cv2.setMouseCallback(self.window_name, self.on_mouse)
def on_color(self, msg):
self.color_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
def on_depth(self, msg):
self.depth_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough')
def on_info(self, msg):
self.K = np.array(msg.k, dtype=np.float64).reshape(3, 3)
def pixel_to_xyz(self, u, v):
if self.depth_img is None or self.K is None:
return None
h, w = self.depth_img.shape[:2]
if not (0 <= u < w and 0 <= v < h):
return None
patch = self.depth_img[max(0, v - 2):min(h, v + 3), max(0, u - 2):min(w, u + 3)].astype(np.float32)
valid = patch[patch > 0]
if valid.size == 0:
return None
z = np.median(valid) * self.depth_scale
fx = self.K[0, 0]
fy = self.K[1, 1]
cx = self.K[0, 2]
cy = self.K[1, 2]
x = (u - cx) * z / fx
y = (v - cy) * z / fy
return float(x), float(y), float(z)
def clamp_servo(self, value):
value = int(round(value))
if value < self.servo_min:
value = self.servo_min
if value > self.servo_max:
value = self.servo_max
return value
def on_mouse(self, event, x, y, flags, param):
if event != cv2.EVENT_LBUTTONDOWN:
return
xyz = self.pixel_to_xyz(x, y)
if xyz is None:
print('[WARN] 无有效深度')
return
X, Y, Z = xyz
# 模型输出顺序：6,5,4,3
cmd_raw = features(X, Y) @ self.W
cmd_raw = cmd_raw.tolist()
# 先取整并限幅
cmd_int = [self.clamp_servo(v) for v in cmd_raw]
# 转成 {舵机ID: 值}
servo_map = {}
for sid, val in zip(self.model_servo_order, cmd_int):
servo_map[sid] = val
# 按你希望的顺序打印：3,4,5,6
ordered_pairs = [(sid, servo_map[sid]) for sid in self.print_servo_order]
print(f'\n点击像素: ({x}, {y})')
print(f'相机坐标: X={X:.4f}, Y={Y:.4f}, Z={Z:.4f}')
print('预测机械臂命令向量(按模型顺序 6,5,4,3):',
', '.join([str(servo_map[sid]) for sid in self.model_servo_order]))
print('舵机列表:', ','.join([f'{sid}:{val}' for sid, val in ordered_pairs]))
def spin_loop(self):
while rclpy.ok():
rclpy.spin_once(self, timeout_sec=0.03)
if self.color_img is not None:
cv2.imshow(self.window_name, self.color_img)
if (cv2.waitKey(1) & 0xFF) == ord('q'):
break
cv2.destroyAllWindows()
def main():
rclpy.init()
node = PlaneCmdPredictor()
try:
node.spin_loop()
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

310
arm_plane_calib/readme.md Normal file
View file

@ -0,0 +1,310 @@
# arm_plane_calib 使用说明
## 1. 当前机器的实际设备映射
根据现场枚举结果，当前机器建议按下面的映射理解和使用：
### 1.1 串口设备
- **机械臂控制口**：`/dev/ttyUSB1`
- **机械臂默认波特率**：`9600`
现场串口枚举：
```bash
root@ubuntu:~# ls /dev/ttyUSB*
/dev/ttyUSB0 /dev/ttyUSB1
```
说明：
- `ttyUSB0` 当前用于底盘
- `ttyUSB1` 当前用于机械臂
### 1.2 深度相机
- **深度相机型号**：Orbbec Gemini 2
- **系统节点**：`/dev/video1`
但本工程里的深度数据 **不是直接按普通 UVC 摄像头去读 `/dev/video1`**，而是通过 `orbbec_camera` 驱动发布 ROS 2 话题：
- `/camera/color/image_raw`
- `/camera/depth/image_raw`
- `/camera/depth/camera_info`
所以在做标定前，需要先把 Gemini 2 的 ROS 话题跑起来。
---
## 2. 目录说明
```bash
~/arm_plane_calib
├── biaodi.png # 标定示意图
├── collect_plane_calib.py # 采样脚本
├── depth_click_to_learm_cmd_node.py # 点击深度图 -> 预测舵机值 -> 可选自动发送
├── fit_plane_calib.py # 拟合脚本
├── learm_test.py # 机械臂串口测试脚本
├── model.yaml # 拟合后的模型参数
├── predict_plane_cmd.py # 点击图像预测舵机命令
├── readme.md # 原始说明
└── samples.yaml # 采样数据
```
---
## 3. 平面标定的目的
平面标定的核心目的是：
> 当你在相机画面中点击一个桌面目标点时，系统能够根据深度相机测得的三维信息，预测出一组机械臂舵机控制值，使机械臂末端运动到该目标点上方。
当前这套标定使用的是：
- 输入特征：`x_cam`, `y_cam`
- 拟合模型：二次多项式
- 特征顺序：`[1, x, y, x², x*y, y²]`
注意：当前拟合 **不直接使用 `z_cam` 做建模**，但会记录 `z_cam`，并在 `model.yaml` 中给出 `z_ref_median` 作为参考。
---
## 4. 标定前准备
### 4.1 先启动 Gemini 2
```bash
cd ~/dev_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch origintank_bringup orbbec_gemini2.launch.py
```
### 4.2 再进入标定目录
```bash
cd ~/arm_plane_calib
```
---
## 5. 脚本说明
### 5.1 `collect_plane_calib.py` —— 采样脚本
作用：
- 订阅：
- `/camera/color/image_raw`
- `/camera/depth/image_raw`
- `/camera/depth/camera_info`
- 鼠标点击图像中的标定点
- 自动计算该点的相机坐标 `X_cam, Y_cam, Z_cam`
- 终端手动输入对应的机械臂命令向量
- 自动保存到 `samples.yaml`
- 采满设定数量后自动退出
默认参数要点：
- `save_path = samples.yaml`
- `depth_scale = 0.001`
- `min_depth_m = 0.05`
- `max_depth_m = 1.50`
- `median_kernel = 5`
- `target_count = 9`
运行：
```bash
cd ~/arm_plane_calib
python3 collect_plane_calib.py
```
采样时终端输入示例：
```bash
520,610,430,500
```
这 4 维命令向量的具体含义，需要与你的模型输出顺序保持一致。
---
### 5.2 `fit_plane_calib.py` —— 拟合脚本
作用：
- 读取 `samples.yaml`
- 构造二次多项式特征
- 对机械臂命令向量做最小二乘拟合
- 输出 `model.yaml`
- 同时打印每一维的 `MAE` 和 `RMSE`
运行：
```bash
cd ~/arm_plane_calib
python3 fit_plane_calib.py samples.yaml model.yaml
```
要求：
- 至少需要 `6` 个样本点才能拟合
- 实际建议 `9~16` 个点
- 点位尽量铺满工作区，不要只集中在中间
---
### 5.3 `predict_plane_cmd.py` —— 点击图像预测舵机值
作用：
- 运行时点击图像任意位置
- 自动求取该像素点对应的相机三维坐标
- 调用 `model.yaml` 预测机械臂命令向量
- 在终端输出预测结果
运行：
```bash
cd ~/arm_plane_calib
python3 predict_plane_cmd.py
```
当前脚本中的关键约定：
- 模型输出顺序固定为：**6、5、4、3**
- 终端打印时会整理为：**3、4、5、6**
- 当前默认舵机限幅范围：`125 ~ 875`
示例输出：
```text
点击像素: (694, 230)
相机坐标: X=0.0267, Y=-0.0681, Z=0.3600
预测机械臂命令向量(按模型顺序 6,5,4,3): 501, 874, 385, 393
舵机列表: 3:393,4:385,5:874,6:501
```
---
### 5.4 `depth_click_to_learm_cmd_node.py` —— 点击深度图后可直接发送控制
作用：
- 订阅深度图、彩色图和相机内参
- 点击图像后自动预测机械臂命令向量
- 可按参数决定是否自动通过串口发送给机械臂
关键默认参数：
- `serial_port = /dev/ttyUSB0`
- `baud_rate = 9600`
- `move_time_ms = 800`
- `servo_ids = [6, 5, 4, 3]`
- `servo_min = [125, 125, 125, 125]`
- `servo_max = [875, 875, 875, 875]`
- `auto_send = True`
**注意：这个脚本源码默认串口仍然写的是 `/dev/ttyUSB0`，与你当前机器的机械臂实际口 `/dev/ttyUSB1` 不一致。**
因此当前机器使用时，建议显式传参：
```bash
cd ~/arm_plane_calib
python3 depth_click_to_learm_cmd_node.py --ros-args -p serial_port:=/dev/ttyUSB1 -p baud_rate:=9600
```
---
### 5.5 `learm_test.py` —— 机械臂串口测试脚本
作用：
- 测试串口是否能连上机械臂
- 查询固件
- 回读舵机位置
- 发初始位姿命令
- 控制单个或多个舵机
当前脚本 `main()` 里默认写的是：
- `port = /dev/ttyUSB0`
- `baudrate = 9600`
但你当前机器的机械臂实际连接为 **`/dev/ttyUSB1`**，因此使用前建议先把源码最后一行改成：
```python
arm = LeArmSerial(port="/dev/ttyUSB1", baudrate=9600)
```
再运行：
```bash
cd ~/arm_plane_calib
python3 learm_test.py
```
---
## 6. 九点标定推荐流程
### 6.1 布点建议
建议用 3×3 共 9 个点，尽量铺满机械臂实际抓取区域：
```text
1 2 3
4 5 6
7 8 9
```
### 6.2 启动 Gemini 2
```bash
cd ~/dev_ws
source /opt/ros/humble/setup.bash
source install/setup.bash
ros2 launch origintank_bringup orbbec_gemini2.launch.py
```
### 6.3 运行采样脚本
```bash
cd ~/arm_plane_calib
python3 collect_plane_calib.py
```
### 6.4 对每个点重复以下动作
1. 鼠标点击图像中的桌面标定点
2. 手动把机械臂末端移动到该点正上方
3. 在终端输入这一点的机械臂命令向量
4. 脚本自动保存到 `samples.yaml`
例如输入：
```bash
520,610,430,500
```
说明：
- 当前标定默认以 **4 维命令向量** 为例
- 夹爪开合通常不参与平面标定
- 抓取时再单独附加夹爪动作即可
### 6.5 完成后拟合模型
```bash
cd ~/arm_plane_calib
python3 fit_plane_calib.py samples.yaml model.yaml
```
### 6.6 预测测试
```bash
cd ~/arm_plane_calib
python3 predict_plane_cmd.py
```

92
arm_plane_calib/samples.yaml Normal file
View file

@ -0,0 +1,92 @@
samples:
- u: 582
v: 240
x_cam: -0.03145534404311617
y_cam: -0.0625876597787623
z_cam: 0.358
arm_cmd:
- 540.0
- 885.0
- 421.0
- 375.0
- u: 681
v: 233
x_cam: 0.01995364206116322
y_cam: -0.06658371695309058
z_cam: 0.36
arm_cmd:
- 505.0
- 884.0
- 369.0
- 401.0
- u: 771
v: 243
x_cam: 0.06592038079659751
y_cam: -0.060522141786992736
z_cam: 0.355
arm_cmd:
- 467.0
- 872.0
- 363.0
- 422.0
- u: 566
v: 316
x_cam: -0.03830266258671575
y_cam: -0.022374943079671503
z_cam: 0.34500000000000003
arm_cmd:
- 514.0
- 847.0
- 284.0
- 419.0
- u: 681
v: 323
x_cam: 0.019066813525111522
y_cam: -0.018825745192638882
z_cam: 0.34400000000000003
arm_cmd:
- 468.0
- 815.0
- 332.0
- 363.0
- u: 761
v: 329
x_cam: 0.05855634668063165
y_cam: -0.015747077324389527
z_cam: 0.342
arm_cmd:
- 513.0
- 879.0
- 242.0
- 498.0
- u: 566
v: 412
x_cam: -0.03674835164116786
y_cam: 0.024512461886783923
z_cam: 0.331
arm_cmd:
- 566.0
- 816.0
- 274.0
- 347.0
- u: 681
v: 419
x_cam: 0.018179984989059823
y_cam: 0.027612575073894863
z_cam: 0.328
arm_cmd:
- 518.0
- 816.0
- 274.0
- 347.0
- u: 780
v: 419
x_cam: 0.06517939133605848
y_cam: 0.027612575073894863
z_cam: 0.328
arm_cmd:
- 450.0
- 843.0
- 384.0
- 265.0
cmd_dim: 4