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.013 PICK_ANGLE_ORIENTATION_INDEX = 4 PICK_ANGLE_SIGN = 1.0 MAX_PICK_ANGLE_DEG = 30.0 # 当前项目只使用一个放置位，仍保留列表结构以兼容 control_core。 place_positions = [[-0.6203, 0.026, 0.1078, 2.9923, 0.0366, -1.4157]] # -------------------- 夹爪与机械臂运动参数 -------------------- GRIPPER_CLOSE_IO = 1 GRIPPER_OPEN_IO = 0 GRIPPER_ACTION_DELAY = 2.0 PICK_POINT_ARRIVAL_DELAY = 1.0 SCISSORS_ENABLED = True # 这里沿用底层 SDK 的弧度制。 ARM_SPEED = 150 * (M_PI / 180) ARM_ACCEL = 120 * (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", "PICK_POINT_ARRIVAL_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()