sunxianghui
114 lines
3.4 KiB
Python
Executable File
114 lines
3.4 KiB
Python
Executable File
#!/usr/bin/env python3
|
|
"""O6 中指 step 控制指令（真机 / 仿真共用，时序完全一致）。
|
|
|
|
时序（默认）：
|
|
pre_wait=3s 保持张开
|
|
hold=3s 中指握紧 (middle u8=0)
|
|
hold=3s 中指张开 (middle u8=255)
|
|
|
|
用法（单独发令，配合录制脚本）：
|
|
export ROS_DOMAIN_ID=31
|
|
python3 tools/middle_step_cmd_pub.py
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import argparse
|
|
import time
|
|
|
|
# 与 run_middle_step_experiment.py 共用，改这里两边一起变
|
|
DEFAULT_PRE_WAIT = 3.0
|
|
DEFAULT_HOLD = 3.0
|
|
DEFAULT_RATE_HZ = 10.0
|
|
FINGER_ROS_IDX = {"index": 2, "middle": 3, "ring": 4, "pinky": 5}
|
|
|
|
|
|
def hold_finger_pose(pub, hand: str, finger: str, u8: float, duration: float, rate_hz: float):
|
|
from sensor_msgs.msg import JointState
|
|
|
|
idx = FINGER_ROS_IDX[finger]
|
|
end = time.perf_counter() + duration
|
|
dt = 1.0 / max(1.0, rate_hz)
|
|
while time.perf_counter() < end:
|
|
msg = JointState()
|
|
msg.position = [255.0, 255.0, 255.0, 255.0, 255.0, 255.0]
|
|
msg.position[idx] = float(u8)
|
|
pub.publish(msg)
|
|
time.sleep(dt)
|
|
|
|
|
|
def run_step_sequence(
|
|
node,
|
|
*,
|
|
hand: str = "left",
|
|
finger: str = "middle",
|
|
pre_wait: float = DEFAULT_PRE_WAIT,
|
|
hold: float = DEFAULT_HOLD,
|
|
rate_hz: float = DEFAULT_RATE_HZ,
|
|
cmd_topic: str | None = None,
|
|
):
|
|
from sensor_msgs.msg import JointState
|
|
|
|
topic = cmd_topic or f"/cb_{hand}_hand_control_cmd"
|
|
pub = node.create_publisher(JointState, topic, 10)
|
|
time.sleep(0.3)
|
|
|
|
t0 = time.perf_counter()
|
|
node.get_logger().info(
|
|
f"step cmd | finger={finger} | pre={pre_wait}s hold={hold}s | topic={topic}"
|
|
)
|
|
|
|
# 阶段0：保持张开
|
|
node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] baseline open (255)")
|
|
hold_finger_pose(pub, hand, finger, 255.0, pre_wait, rate_hz)
|
|
|
|
# 阶段1：握紧
|
|
node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] CLOSE middle u8=0")
|
|
hold_finger_pose(pub, hand, finger, 0.0, hold, rate_hz)
|
|
|
|
# 阶段2：张开
|
|
node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] OPEN middle u8=255")
|
|
hold_finger_pose(pub, hand, finger, 255.0, hold, rate_hz)
|
|
|
|
node.get_logger().info(f"[t={time.perf_counter()-t0:.2f}s] sequence done")
|
|
return t0
|
|
|
|
|
|
def total_duration(pre_wait: float, hold: float, record_after: float = 0.0) -> float:
|
|
return pre_wait + hold + hold + record_after + 1.0
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser(description="O6 middle finger step command publisher")
|
|
parser.add_argument("--hand", default="left", choices=["left", "right"])
|
|
parser.add_argument("--finger", default="middle", choices=list(FINGER_ROS_IDX))
|
|
parser.add_argument("--pre-wait", type=float, default=DEFAULT_PRE_WAIT)
|
|
parser.add_argument("--hold", type=float, default=DEFAULT_HOLD)
|
|
parser.add_argument("--rate", type=float, default=DEFAULT_RATE_HZ)
|
|
parser.add_argument("--cmd-topic", default="")
|
|
args = parser.parse_args()
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
|
|
rclpy.init()
|
|
node = Node("middle_step_cmd_pub")
|
|
try:
|
|
run_step_sequence(
|
|
node,
|
|
hand=args.hand,
|
|
finger=args.finger,
|
|
pre_wait=args.pre_wait,
|
|
hold=args.hold,
|
|
rate_hz=args.rate,
|
|
cmd_topic=args.cmd_topic or None,
|
|
)
|
|
finally:
|
|
node.destroy_node()
|
|
if rclpy.ok():
|
|
rclpy.shutdown()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|