initial

2026-05-06 10:47:14 +08:00
commit e3be8861b8
9 changed files with 4380 additions and 0 deletions
--- a/control.py
+++ b/control.py
@ -0,0 +1,243 @@
+import signal
+from typing import Any, Sequence
+
+import control_core as _core
+
+"""
+对外兼容入口。
+这个文件主要做两件事：
+1. 暴露和旧版本一致的函数名，方便 UI 或外部脚本继续调用；
+2. 在真正进入核心逻辑前，把这里可修改的配置同步到 `control_core.py`。
+"""
+
+
+# -------------------- 设备连接参数 --------------------
+ROBOT_IP = "192.168.192.100"
+ROBOT_PORT = 30004
+M_PI = _core.M_PI
+
+AGV_IP = "192.168.192.100"
+AGV_PORT = 30104
+AGV_SPEED_FORWARD = 0
+AGV_SPEED_STOP = 0.0
+AGV_RUN_DISTANCE = 5.0
+
+
+# -------------------- 运行时长与检测参数 --------------------
+TOTAL_DURATION = 300
+AGV_STOP_TIMEOUT = 10
+AGV_PICK_SETTLE_DELAY = 1.0
+YOLO_MODEL_PATH = "best.pt"
+YOLO_DETECT_CONF = 0.5
+PICK_CONFIDENCE_THRESHOLD = 0.7
+
+
+# -------------------- 位姿与轨迹参数 --------------------
+# Home 是机械臂的初始/复位关节位。
+HOME_JOINTS = [-1.5247, 1.0899, 2.4671, 1.2761, 1.5113, 0.0001]
+
+# 视觉算出的目标点会再叠加这两个偏移：
+# APPROACH_Y_OFFSET 用于先到预抓取点；
+# LIFT_Z_OFFSET 用于修正真正执行抓取时的高度。
+APPROACH_Y_OFFSET = 0.15
+LIFT_Z_OFFSET = 0.035
+PICKUP_X_OFFSET = 0.0
+PICKUP_Y_OFFSET = -0.015
+PICK_ANGLE_ORIENTATION_INDEX = 4
+PICK_ANGLE_SIGN = 1.0
+MAX_PICK_ANGLE_DEG = 30.0
+
+# 当前项目只使用一个放置位，仍保留列表结构以兼容 control_core。
+place_positions = [[-0.6283, 0.0354, -0.0889, 2.8798, 0.02, -1.4346]]
+
+
+# -------------------- 夹爪与机械臂运动参数 --------------------
+GRIPPER_CLOSE_IO = 1
+GRIPPER_OPEN_IO = 0
+GRIPPER_ACTION_DELAY = 2.0
+SCISSORS_ENABLED = True
+
+# 这里沿用底层 SDK 的弧度制。
+ARM_SPEED = 100 * (M_PI / 180)
+ARM_ACCEL = 100 * (M_PI / 180)
+
+
+# -------------------- 参数同步清单 --------------------
+_CONFIG_NAMES = (
+    "ROBOT_IP",
+    "ROBOT_PORT",
+    "AGV_IP",
+    "AGV_PORT",
+    "AGV_SPEED_FORWARD",
+    "AGV_SPEED_STOP",
+    "AGV_RUN_DISTANCE",
+    "TOTAL_DURATION",
+    "AGV_STOP_TIMEOUT",
+    "AGV_PICK_SETTLE_DELAY",
+    "YOLO_MODEL_PATH",
+    "YOLO_DETECT_CONF",
+    "PICK_CONFIDENCE_THRESHOLD",
+    "HOME_JOINTS",
+    "APPROACH_Y_OFFSET",
+    "LIFT_Z_OFFSET",
+    "PICKUP_X_OFFSET",
+    "PICKUP_Y_OFFSET",
+    "PICK_ANGLE_ORIENTATION_INDEX",
+    "PICK_ANGLE_SIGN",
+    "MAX_PICK_ANGLE_DEG",
+    "GRIPPER_CLOSE_IO",
+    "GRIPPER_OPEN_IO",
+    "GRIPPER_ACTION_DELAY",
+    "SCISSORS_ENABLED",
+    "ARM_SPEED",
+    "ARM_ACCEL",
+    "place_positions",
+)
+
+
+# -------------------- 运行状态别名 --------------------
+running = _core.running
+has_tomato = _core.has_tomato
+picking_done = _core.picking_done
+robot_rpc_client = _core.robot_rpc_client
+ui_callback = _core.ui_callback
+
+
+def _sync_config() -> None:
+    # 如果 UI 或外部脚本只改了 control.py，这里会在进入核心逻辑前同步到 control_core。
+    config = {name: globals()[name] for name in _CONFIG_NAMES}
+    if place_positions and isinstance(place_positions[0], (list, tuple)):
+        config["place_positions"] = [list(position) for position in place_positions]
+    else:
+        config["place_positions"] = [list(place_positions)]
+    config["HOME_JOINTS"] = list(HOME_JOINTS)
+    _core.configure(config)
+
+
+def set_ui_callback(callback):
+    global ui_callback
+    ui_callback = callback
+    _core.set_ui_callback(callback)
+
+
+def exampleState(robot_name: str):
+    return _core.exampleState(robot_name)
+
+
+def get_robot_end_effector_pose(robot_name: str):
+    return _core.get_robot_end_effector_pose(robot_name)
+
+
+def exampleInverseK(robot_name: str, pose: Sequence[float], reference_q: Sequence[float]):
+    return _core.exampleInverseK(robot_name, pose, reference_q)
+
+
+def exampleStartup():
+    _sync_config()
+    return _core.exampleStartup()
+
+
+def quaternion_to_rotation_matrix(q: Sequence[float]):
+    return _core.quaternion_to_rotation_matrix(q)
+
+
+def euler_to_rotation_matrix(roll: float, pitch: float, yaw: float):
+    return _core.euler_to_rotation_matrix(roll, pitch, yaw)
+
+
+def get_robot_end_matrix(robot_name: str):
+    return _core.get_robot_end_matrix(robot_name)
+
+
+def camera_to_base(point_cam: Sequence[float], robot_name: str):
+    return _core.camera_to_base(point_cam, robot_name)
+
+
+def check_joint_position(robot_name: str, target_joints: Sequence[float]):
+    return _core.check_joint_position(robot_name, target_joints)
+
+
+def check_tcp_pose(robot_name: str, target_pose: Sequence[float]):
+    return _core.check_tcp_pose(robot_name, target_pose)
+
+
+def waitArrival(impl: Any, target_joints=None, target_pose=None):
+    return _core.waitArrival(impl, target_joints=target_joints, target_pose=target_pose)
+
+
+def control_tool_io(robot_name: str, io_index: int, state: bool):
+    return _core.control_tool_io(robot_name, io_index, state)
+
+
+def get_next_placement():
+    _sync_config()
+    return _core.get_next_placement()
+
+
+def get_placement_index():
+    return _core.get_placement_index()
+
+
+def create_placement_manager():
+    _sync_config()
+    return _core.create_placement_manager()
+
+
+def return_to_home(robot_name: str):
+    _sync_config()
+    return _core.return_to_home(robot_name)
+
+
+def control_robot(robot_name: str, pose: Sequence[float], angle_rad: float):
+    _sync_config()
+    return _core.control_robot(robot_name, pose, angle_rad)
+
+
+def init_camera():
+    _sync_config()
+    return _core.init_camera()
+
+
+def init_tomato_detector(model_path=None):
+    _sync_config()
+    return _core.init_tomato_detector(model_path)
+
+
+def filter_ripe_tomatoes(results, confidence_threshold=None):
+    _sync_config()
+    return _core.filter_ripe_tomatoes(results, confidence_threshold)
+
+
+def draw_tomato_tilt_line(color_image, x1, y1, x2, y2, pixel_x):
+    return _core.draw_tomato_tilt_line(color_image, x1, y1, x2, y2, pixel_x)
+
+
+def fit_red_line(crop_image, x1, y1):
+    return _core.fit_red_line(crop_image, x1, y1)
+
+
+def vision_detection_thread(pipeline, align, depth_intrinsics, model, robot_name: str):
+    _sync_config()
+    return _core.vision_detection_thread(pipeline, align, depth_intrinsics, model, robot_name)
+
+
+def agv_control_thread(agv_client):
+    _sync_config()
+    return _core.agv_control_thread(agv_client)
+
+
+def main():
+    # control.py 本身只做兼容和配置转发，真正业务流程在 control_core.main()。
+    _sync_config()
+    return _core.main()
+
+
+def signal_handler(sig: int, frame: Any):
+    _sync_config()
+    return _core.signal_handler(sig, frame)
+
+
+if __name__ == "__main__":
+    signal.signal(signal.SIGINT, signal_handler)
+    signal.signal(signal.SIGTERM, signal_handler)
+    main()