95 lines
4.5 KiB
Python
95 lines
4.5 KiB
Python
#!/usr/bin/env python3
|
||
"""真机右臂:缓缓垂直上抬 Δz 米(姿态保持不变),MoveL 直线插值。第一次动真机用——慢速、抬升前打印计划并等你确认。
|
||
|
||
安全设计:
|
||
· 姿态用 forward_kinematics 拿当前 euler(驱动 RPY 弧度约定)原样保留,零欧拉歧义。
|
||
· 目标位置 = 当前 pose_states 位置 + (0,0,Δz);抬升前交叉校验 FK 位置≈pose_states(<2cm),不一致直接中止(防法兰/工具系错配)。
|
||
· 只发一次 MoveL、block=False;不确认(输入 y)不发指令。
|
||
· 随时急停(另开终端):
|
||
source /opt/ros/jazzy/setup.bash; source $A7_BRIDGE_SETUP
|
||
ros2 service call /robot1/right_arm/set_emergency_stop lbot_arm_interfaces/srv/SetEmergency "{emergency: true}"
|
||
|
||
运行:
|
||
source /opt/ros/jazzy/setup.bash
|
||
source $A7_BRIDGE_SETUP
|
||
python3 scripts/real_raise.py --dz 0.10 --speed 0.3
|
||
"""
|
||
import argparse, sys
|
||
import rclpy
|
||
from rclpy.node import Node
|
||
from sensor_msgs.msg import JointState
|
||
from geometry_msgs.msg import PoseStamped, Vector3
|
||
from lbot_arm_interfaces.srv import ForwardKinematics, MoveL
|
||
|
||
NS = "/robot1/right_arm"
|
||
NAMES = [f"R{i}_joint" for i in range(1, 8)]
|
||
|
||
|
||
def wait_msg(node, topic, mtype, timeout=5.0):
|
||
box = {}
|
||
sub = node.create_subscription(mtype, topic, lambda m: box.setdefault("m", m), 10)
|
||
t0 = node.get_clock().now()
|
||
while rclpy.ok() and "m" not in box:
|
||
rclpy.spin_once(node, timeout_sec=0.1)
|
||
if (node.get_clock().now() - t0).nanoseconds / 1e9 > timeout:
|
||
break
|
||
node.destroy_subscription(sub)
|
||
return box.get("m")
|
||
|
||
|
||
def call(node, client, req, timeout=8.0):
|
||
if not client.wait_for_service(timeout_sec=timeout):
|
||
raise SystemExit(f"服务 {client.srv_name} 不可用")
|
||
fut = client.call_async(req)
|
||
rclpy.spin_until_future_complete(node, fut, timeout_sec=timeout)
|
||
return fut.result()
|
||
|
||
|
||
def main():
|
||
ap = argparse.ArgumentParser()
|
||
ap.add_argument("--dz", type=float, default=0.10, help="上抬高度(米,+z=远离桌面)")
|
||
ap.add_argument("--speed", type=float, default=0.3, help="rad/s,建议 0.2~0.5")
|
||
ap.add_argument("--accel", type=float, default=0.3)
|
||
ap.add_argument("--yes", action="store_true", help="跳过确认(不建议第一次用)")
|
||
a = ap.parse_args()
|
||
|
||
rclpy.init()
|
||
node = rclpy.create_node("real_raise")
|
||
js = wait_msg(node, f"{NS}/joint_states", JointState)
|
||
ps = wait_msg(node, f"{NS}/pose_states", PoseStamped)
|
||
if js is None or ps is None:
|
||
raise SystemExit("读不到 joint_states / pose_states——确认桥接在跑、话题有数据")
|
||
jmap = dict(zip(js.name, js.position))
|
||
joints = [float(jmap[n]) for n in NAMES]
|
||
|
||
fk_cli = node.create_client(ForwardKinematics, f"{NS}/forward_kinematics")
|
||
fk = call(node, fk_cli, ForwardKinematics.Request(joints=[float(x) for x in joints]))
|
||
if not fk or not fk.success:
|
||
raise SystemExit("FK 失败")
|
||
p, e = fk.position, fk.euler
|
||
cur = ps.pose.position
|
||
# 交叉校验:FK 位置 ≈ pose_states 位置(防法兰/工具系错配)
|
||
d = ((p.x - cur.x)**2 + (p.y - cur.y)**2 + (p.z - cur.z)**2) ** 0.5
|
||
print(f"当前关节(rad): {[round(x,3) for x in joints]}")
|
||
print(f"pose_states 位置: ({cur.x:.3f},{cur.y:.3f},{cur.z:.3f})")
|
||
print(f"FK 位置: ({p.x:.3f},{p.y:.3f},{p.z:.3f}) euler(rpy rad)=({e.x:.3f},{e.y:.3f},{e.z:.3f})")
|
||
print(f"FK vs pose_states 位置差 = {d*1000:.1f} mm")
|
||
if d > 0.02:
|
||
raise SystemExit(f"⚠ FK 与 pose_states 位置差 {d*1000:.0f}mm >20mm,可能坐标系不一致——中止,不发运动指令")
|
||
tgt = Vector3(x=cur.x, y=cur.y, z=cur.z + a.dz)
|
||
print(f"\n计划: 右臂末端从 z={cur.z:.3f} 直线上抬到 z={tgt.z:.3f} (+{a.dz*100:.0f}cm, +z=远离桌面), 姿态不变")
|
||
print(f" speed={a.speed} rad/s accel={a.accel} block=False")
|
||
if not a.yes:
|
||
if input("确认发送?输入 y 回车执行,其它任意键取消: ").strip().lower() != "y":
|
||
print("已取消,未发指令。"); return
|
||
mv = node.create_client(MoveL, f"{NS}/move_linear")
|
||
req = MoveL.Request(position=tgt, euler=e, speed=float(a.speed), acce=float(a.accel), block=False)
|
||
res = call(node, mv, req)
|
||
print(f"MoveL 指令已发送,success={getattr(res,'success',None)}")
|
||
print("急停(另开终端): ros2 service call /robot1/right_arm/set_emergency_stop lbot_arm_interfaces/srv/SetEmergency \"{emergency: true}\"")
|
||
node.destroy_node(); rclpy.shutdown()
|
||
|
||
|
||
if __name__ == "__main__":
|
||
main()
|